Skip to content

Commit

Permalink
Feature/add pandar 64 (#4)
Browse files Browse the repository at this point in the history 
* Pandar64. Add support

Signed-off-by: amc-nu <[email protected]>

* revert 40p config file to its default

Signed-off-by: amc-nu <[email protected]>
  • Loading branch information
@amc-nu
amc-nu authored Jul 26, 2021
1 parent 50e4e9d commit e45a92f
Show file tree
Hide file tree
Showing 7 changed files with 524 additions and 0 deletions.
1 change: 1 addition & 0 deletions pandar_pointcloud/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@ add_library(pandar_cloud
src/lib/decoder/pandar40_decoder.cpp
src/lib/decoder/pandar_qt_decoder.cpp
src/lib/decoder/pandar_xt_decoder.cpp
src/lib/decoder/pandar64_decoder.cpp
)
target_link_libraries(pandar_cloud
${catkin_LIBRARIES}
Expand Down
65 changes: 65 additions & 0 deletions pandar_pointcloud/config/64.csv
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,65 @@
Laser id,Elevation,Azimuth
1,14.9,-1.042
2,11.05,-1.042
3,8.077,-1.042
4,5.075,-1.042
5,3.058,-1.042
6,2.046,-1.042
7,1.878,1.042
8,1.706,3.125
9,1.54,5.208
10,1.369,-5.208
11,1.202,-3.125
12,1.031,-1.042
13,0.864,1.042
14,0.693,3.125
15,0.526,5.208
16,0.355,-5.208
17,0.187,-3.125
18,0.018,-1.042
19,-0.151,1.042
20,-0.319,3.125
21,-0.49,5.208
22,-0.657,-5.208
23,-0.827,-3.125
24,-0.995,-1.042
25,-1.166,1.042
26,-1.333,3.125
27,-1.504,5.208
28,-1.67,-5.208
29,-1.842,-3.125
30,-2.01,-1.042
31,-2.18,1.042
32,-2.347,3.125
33,-2.518,5.208
34,-2.682,-5.208
35,-2.855,-3.125
36,-3.022,-1.042
37,-3.192,1.042
38,-3.357,3.125
39,-3.53,5.208
40,-3.694,-5.208
41,-3.866,-3.125
42,-4.032,-1.042
43,-4.203,1.042
44,-4.367,3.125
45,-4.54,5.208
46,-4.702,-5.208
47,-4.874,-3.125
48,-5.039,-1.042
49,-5.211,1.042
50,-5.373,3.125
51,-5.547,5.208
52,-5.708,-5.208
53,-5.88,-3.125
54,-6.043,-1.042
55,-7.045,-1.042
56,-8.041,-1.042
57,-9.042,-1.042
58,-10.027,-1.042
59,-11.014,-1.042
60,-11.988,-1.042
61,-12.956,-1.042
62,-13.912,-1.042
63,-18.871,-1.042
64,-24.879,-1.042
70 changes: 70 additions & 0 deletions pandar_pointcloud/include/pandar_pointcloud/decoder/pandar64.hpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,70 @@
#pragma once
/**
* Pandar 64
*/
#include <cstdint>
namespace pandar_pointcloud
{
namespace pandar64
{
// Head
constexpr size_t HEAD_SIZE = 8;
// Body
constexpr size_t BLOCK_NUM = 6;
constexpr size_t BLOCK_HEADER_AZIMUTH = 2;
constexpr size_t UNIT_NUM = 64;
constexpr size_t UNIT_SIZE = 3;
constexpr size_t BLOCK_SIZE = UNIT_SIZE * UNIT_NUM + BLOCK_HEADER_AZIMUTH;
constexpr size_t BODY_SIZE = BLOCK_SIZE * BLOCK_NUM;
// Tail
constexpr size_t RESERVED_SIZE = 8;
constexpr size_t HIGH_TEMPERATURE = 1;
constexpr size_t ENGINE_VELOCITY = 2;
constexpr size_t TIMESTAMP_SIZE = 4;
constexpr size_t RETURN_SIZE = 1;//echo
constexpr size_t FACTORY_SIZE = 1;
constexpr size_t UTC_SIZE = 6;
constexpr size_t PACKET_TAIL_SIZE = 26;
constexpr size_t PACKET_TAIL_WITHOUT_UDPSEQ_SIZE = 22;

// All
constexpr size_t PACKET_SIZE = HEAD_SIZE + BODY_SIZE + PACKET_TAIL_SIZE;
constexpr size_t PACKET_WITHOUT_UDPSEQ_SIZE = HEAD_SIZE + BODY_SIZE + PACKET_TAIL_WITHOUT_UDPSEQ_SIZE;

constexpr uint32_t STRONGEST_RETURN = 0x37;
constexpr uint32_t LAST_RETURN = 0x38;
constexpr uint32_t DUAL_RETURN = 0x39;

struct Header
{
uint16_t sob; // 0xEEFF 2bytes
int8_t chLaserNumber; // laser number 1byte
int8_t chBlockNumber; // block number 1byte
int8_t chReturnType; // return mode 1 byte when dual return 0-Single Return
// 1-The first block is the 1 st return.
// 2-The first block is the 2 nd return
int8_t chDisUnit; // Distance unit, 4mm
};

struct Unit
{
double distance;
uint16_t intensity;
};

struct Block
{
uint16_t azimuth; // packet angle,Azimuth = RealAzimuth * 100
Unit units[UNIT_NUM];
};

struct Packet
{
Header header;
Block blocks[BLOCK_NUM];
uint32_t usec; // ms
uint32_t return_mode;
tm t;
};
} // namespace pandar_64
} // namespace pandar_pointcloud
71 changes: 71 additions & 0 deletions pandar_pointcloud/include/pandar_pointcloud/decoder/pandar64_decoder.hpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,71 @@
#pragma once

#include <array>
#include "pandar_pointcloud/calibration.hpp"
#include "packet_decoder.hpp"
#include "pandar64.hpp"

namespace pandar_pointcloud
{
namespace pandar64
{
class Pandar64Decoder : public PacketDecoder
{
public:
enum class ReturnMode : int8_t
{
DUAL,
STRONGEST,
LAST,
};
enum ReturnType : int8_t
{
INVALID = 0,
SINGLE_STRONGEST,
SINGLE_LAST,
DUAL_FIRST,
DUAL_LAST,
DUAL_ONLY,
};

explicit Pandar64Decoder(Calibration& calibration,
float scan_phase = 0.0f,
double dual_return_distance_threshold = 0.1,
ReturnMode return_mode = ReturnMode::DUAL);

void unpack(const pandar_msgs::PandarPacket& raw_packet) override;

PointXYZIRADT build_point(int block_id, int unit_id, int8_t return_type);

bool hasScanned() override;

PointcloudXYZIRADT getPointcloud() override;

private:
bool parsePacket(const pandar_msgs::PandarPacket& raw_packet);

PointcloudXYZIRADT convert(int block_id);

PointcloudXYZIRADT convert_dual(int block_id);

std::array<float, UNIT_NUM> elev_angle_{};
std::array<float, UNIT_NUM> azimuth_offset_{};

std::array<float, UNIT_NUM> firing_offset_{};
std::array<float, BLOCK_NUM> block_offset_single_{};
std::array<float, BLOCK_NUM> block_offset_dual_{};

ReturnMode return_mode_;
double dual_return_distance_threshold_;
Packet packet_{};

PointcloudXYZIRADT scan_pc_;
PointcloudXYZIRADT overflow_pc_;

uint16_t scan_phase_;
int last_phase_;
bool has_scanned_;
};

} // namespace pandar_qt
} // namespace pandar_pointcloud
43 changes: 43 additions & 0 deletions pandar_pointcloud/launch/pandar64.launch
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,43 @@
<launch>
<arg name="launch_driver" default="false" />

<arg name="pcap" default=""/>
<arg name="device_ip" default="192.168.1.201"/>
<arg name="lidar_port" default="2368"/>
<arg name="gps_port" default="10110"/>
<arg name="scan_phase" default="0"/>
<arg name="return_mode" default="Dual"/>
<arg name="dual_return_distance_threshold" default="0.1"/>
<arg name="model" default="Pandar64"/>
<arg name="frame_id" default="pandar"/>
<arg name="calibration" default="$(find pandar_pointcloud)/config/64.csv"/>
<arg name="manager" default="pandar_nodelet_manager"/>

<!-- nodelet manager -->
<node pkg="nodelet" type="nodelet" name="$(arg manager)" args="manager" output="screen"/>

<!-- pandar driver -->
<node pkg="nodelet" type="nodelet" name="$(arg manager)_driver"
args="load pandar_driver/DriverNodelet $(arg manager)" if="$(arg launch_driver)" output="screen">
<param name="pcap" type="string" value="$(arg pcap)"/>
<param name="device_ip" type="string" value="$(arg device_ip)"/>
<param name="lidar_port" type="int" value="$(arg lidar_port)"/>
<param name="gps_port" type="int" value="$(arg gps_port)"/>
<param name="scan_phase" type="double" value="$(arg scan_phase)"/>
<param name="model" type="string" value="$(arg model)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
</node>

<!-- pandar cloud -->
<node pkg="nodelet" type="nodelet" name="$(arg manager)_cloud"
args="load pandar_pointcloud/CloudNodelet $(arg manager)" output="screen">
<remap from="pandar_points" to="pointcloud_raw" />
<remap from="pandar_points_ex" to="pointcloud_raw_ex" />
<param name="scan_phase" type="double" value="$(arg scan_phase)"/>
<param name="return_mode" type="string" value="$(arg return_mode)"/>
<param name="dual_return_distance_threshold" type="double" value="$(arg dual_return_distance_threshold)"/>
<param name="model" type="string" value="$(arg model)"/>
<param name="device_ip" type="string" value="$(arg device_ip)"/>
<param name="calibration" type="string" value="$(arg calibration)"/>
</node>
</launch>
Loading

0 comments on commit e45a92f

Please sign in to comment.