Skip to content

Commit

Permalink
velodyne_packet: Restructure package to avoid code duplication.
Browse files Browse the repository at this point in the history
  • Loading branch information
@judav25
judav25 committed Aug 8, 2024
1 parent bc1ea88 commit 3d4d3da
Show file tree
Hide file tree
Showing 4 changed files with 415 additions and 91 deletions.
2 changes: 1 addition & 1 deletion velodyne_packet_pointcloud/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ find_package(catkin REQUIRED COMPONENTS
${${PROJECT_NAME}_CATKIN_DEPS}
)


find_package(Eigen3 REQUIRED)

# Resolve system dependency on yaml-cpp, which apparently does not
# provide a CMake find_package() module.
Expand Down
77 changes: 60 additions & 17 deletions velodyne_packet_pointcloud/include/velodyne_packet_pointcloud/unpacker.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,33 +15,76 @@
#include <velodyne_pointcloud/rawdata.h>

namespace velodyne_packet_pointcloud
{
{ ;

static const std::string XYZIRT_TYPE = "XYZIRT";
static const std::string ORGANIZED_TYPE = "ORGANIZED";
static const std::string EXTENDED_TYPE = "EXTENDED";
static const std::string EXTENDEDCONF_TYPE = "EXTENDEDCONF";

class Unpacker
class Unpacker : public velodyne_rawdata::DataContainerBase
{
public:
Unpacker(
ros::NodeHandle node,
ros::NodeHandle private_nh,
std::string const &node_name = ros::this_node::getName());
static const std::string XYZIRT_TYPE;
static const std::string EXTENDED_TYPE;
static const std::string EXTENDEDCONF_TYPE;

Unpacker(ros::NodeHandle &nh, ros::NodeHandle &private_nh, const std::string ethernet_msgs_topic, double max_range, const double min_range,
const std::string &target_frame, const std::string &fixed_frame, const std::string &sensor_frame,
unsigned int num_lasers, // determines the width of the cloud
unsigned int points_per_packet);

Unpacker() = delete;

void start_listening(ros::NodeHandle &nh, std::string ethernet_msgs_topic);


~Unpacker() = default;

void addPoint(float x, float y, float z, const uint16_t ring,
const uint16_t azimuth, const float distance,
const float intensity, const float time) override;

void addPoint(float x, float y, float z, const uint16_t ring,
const uint16_t azimuth, const float distance,
const float intensity, const float time,
const uint32_t sub_segment, const uint16_t rotation_segment,
const uint16_t firing_bin, const uint8_t laser_id,
const uint8_t first_return_flag) override;

~Unpacker()
{
}
void addPoint_with_confidence(float x, float y, float z, const uint16_t ring,
const uint16_t azimuth, const float distance,
const float intensity, const float time,
const uint32_t sub_segment,
const uint16_t rotation_segment,
const uint16_t firing_bin, const uint8_t laser_id,
const uint8_t first_return_flag,
const uint8_t drop,
const uint8_t retro_shadow,
const uint8_t range_limited,
const uint8_t retro_ghost,
const uint8_t interference,
const uint8_t sun_lvl,
const uint8_t confidence) override;

void newLine() override;

private:

void processEthernetMsgs(const ethernet_msgs::PacketConstPtr &ethernet_msg);
void processPacket(const velodyne_msgs::VelodynePacket &packetMsg,uint8_t mode);
void processPacket(const velodyne_msgs::VelodynePacket &packetMsg, uint8_t mode);


ros::Publisher output_;
ros::Publisher output_ret_mode_;
ros::Subscriber velodyne_ethernet_msgs_;
std::string input_ethernet_msgs_topic; ///< topic name for ethernet_msgs
double _rpm{600.0};


unsigned int points_per_packet;
std::string model;
std::shared_ptr<velodyne_rawdata::RawData> raw_data_ptr_;

sensor_msgs::PointCloud2Iterator<float> iter_x, iter_y, iter_z, iter_distance, iter_time;
sensor_msgs::PointCloud2Iterator<uint32_t> iter_sub_segment;
sensor_msgs::PointCloud2Iterator<uint16_t> iter_rotation_segment, iter_azimuth, iter_firing_bin, iter_ring;
sensor_msgs::PointCloud2Iterator<uint8_t> iter_intensity, iter_laser_id, iter_first_ret;



};

Expand Down
130 changes: 127 additions & 3 deletions velodyne_packet_pointcloud/src/packetpc_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,14 +3,138 @@
//
#include <ros/ros.h>
#include "velodyne_packet_pointcloud/unpacker.h"
#include <ros/package.h>

int main(int argc, char* argv[])
int main(int argc, char *argv[])
{
ros::init(argc, argv, "velodyne_packet_pointcloud");

auto nh = ros::NodeHandle();
auto private_nh = ros::NodeHandle("~");
std::string sensor_frame;
std::string target_frame; ///< output frame of final point cloud
std::string fixed_frame; ///< world fixed frame for ego motion compenstation
std::string cloud_type; ///< selects the type of point cloud to use as output
std::string input_ethernet_msgs_topic; ///< topic name for ethernet_msgs
std::string model;
double rpm;
float max_range;
float min_range;
std::string calibrationFile; ///< calibration file name

if (!private_nh.getParam("model", model))
{
model = std::string("64E");
ROS_ERROR("No Velodyne Sensor Model specified using default %s!", model.c_str());

}
if (! private_nh.getParam("rpm", rpm))
{
rpm = 600.0;
ROS_ERROR("No Velodyne RPM specified using default %f!", rpm);
}


// get path to angles.config file for this device
if (!private_nh.getParam("calibration", calibrationFile))
{
ROS_ERROR_STREAM("No calibration angles specified! Using test values!");

// have to use something: grab unit test version as a default
std::string pkgPath = ros::package::getPath("velodyne_pointcloud");
calibrationFile = pkgPath + "/params/64e_utexas.yaml";
}


double _rpm{600.0};

if (!private_nh.getParam("input_ethernet_msgs_topic", input_ethernet_msgs_topic))
{
input_ethernet_msgs_topic = "";
}
ROS_WARN("Using %s as ethernet packages topic", input_ethernet_msgs_topic.c_str());
if (!private_nh.getParam("rpm", _rpm))
{
_rpm = 600.0;
ROS_ERROR("No Velodyne RPM specified using default %f!", _rpm);
}

if (!private_nh.getParam("sensor_frame", sensor_frame))
{
sensor_frame = "sensor/lidar/vls128_roof";
ROS_ERROR("No sensor_frame specified using default %s!", sensor_frame.c_str());
}
if (!private_nh.getParam("target_frame", target_frame))
{
target_frame = "";
ROS_ERROR("No target_frame specified using default %s!", target_frame.c_str());
}
if (!private_nh.getParam("fixed_frame", fixed_frame))
{
fixed_frame = "";
ROS_ERROR("No fixed_frame specified using default %s!", fixed_frame.c_str());
}
if (!private_nh.getParam("max_range", max_range))
{
max_range = 300.0;
ROS_ERROR("No max_range specified using default %f!", max_range);
}
if (!private_nh.getParam("min_range", min_range))
{
min_range = 2.0;
ROS_ERROR("No min_range specified using default %f!", min_range);
}

if (!private_nh.getParam("cloud_type", cloud_type))
{
cloud_type = velodyne_packet_pointcloud::Unpacker::EXTENDED_TYPE;
ROS_ERROR("No cloud type specified using default %s!", cloud_type.c_str());
}

// create conversion class, which subscribes to raw data
velodyne_packet_pointcloud::Unpacker unpacker(ros::NodeHandle(),
ros::NodeHandle("~"));
std::unique_ptr<velodyne_rawdata::DataContainerBase> ptr_unpacker;

if (cloud_type == velodyne_packet_pointcloud::Unpacker::XYZIRT_TYPE)
{
// Todo implement unpacker for this kind of cloud
return 0;
}
else
{
if (cloud_type == velodyne_packet_pointcloud::Unpacker::EXTENDED_TYPE)
{
ptr_unpacker = std::make_unique<velodyne_packet_pointcloud::Unpacker>
(nh, private_nh, input_ethernet_msgs_topic,
max_range,
min_range,
target_frame,
fixed_frame,
sensor_frame,
1, // determines the width of the cloud
0); // points per packet (gets overwritten once there is data)
}
else
{
if (cloud_type == velodyne_packet_pointcloud::Unpacker::EXTENDEDCONF_TYPE)
{
// Todo implement unpacker for this kind of cloud
return 0;
}
else
{
ptr_unpacker = std::make_unique<velodyne_packet_pointcloud::Unpacker>
(nh, private_nh, input_ethernet_msgs_topic,
max_range,
min_range,
target_frame,
fixed_frame,
sensor_frame,
1, // determines the width of the cloud
0); // points per packet (gets overwritten once there is data)

}
}
}

// handle callbacks until shut down
ros::spin();
Expand Down
Loading

0 comments on commit 3d4d3da

Please sign in to comment.