Skip to content

Commit

Permalink
Initial working version
Browse files Browse the repository at this point in the history 
Signed-off-by: amc-nu <[email protected]>
  • Loading branch information
@amc-nu
amc-nu committed Jul 5, 2022
1 parent 33ec839 commit a565ae7
Show file tree
Hide file tree
Showing 6 changed files with 268 additions and 192 deletions.
66 changes: 33 additions & 33 deletions pandar_pointcloud/config/xtm.csv
View file
Original file line number Diff line number Diff line change
@@ -1,33 +1,33 @@
Channel,Elevation,Azimuth,,,
1,19.5,0,,,
2,18.2,0,,,
3,16.9,0,,,
4,15.6,0,,,
5,14.3,0,,,
6,13.0,0,,,
7,11.7,0,,,
8,10.4,0,,,
9,9.1,0,,,
10,7.8,0,,,
11,6.5,0,,,
12,5.2,0,,,
13,3.9,0,,,
14,2.6,0,,,
15,1.3,0,,,
16,0.0,0,,,
17,-1.3,0,,,
18,-2.6,0,,,
19,-3.9,0,,,
20,-5.2,0,,,
21,-6.5,0,,,
22,-7.8,0,,,
23,-9.1,0,,,
24,-10.4,0,,,
25,-11.7,0,,,
26,-13.0,0,,,
27,-14.3,0,,,
28,-15.6,0,,,
29,-16.9,0,,,
30,-18.2,0,,,
31,-19.5,0,,,
32,-20.8,0,,,
Channel,Elevation,Azimuth
1,19.5,0
2,18.2,0
3,16.9,0
4,15.6,0
5,14.3,0
6,13.0,0
7,11.7,0
8,10.4,0
9,9.1,0
10,7.8,0
11,6.5,0
12,5.2,0
13,3.9,0
14,2.6,0
15,1.3,0
16,0.0,0
17,-1.3,0
18,-2.6,0
19,-3.9,0
20,-5.2,0
21,-6.5,0
22,-7.8,0
23,-9.1,0
24,-10.4,0
25,-11.7,0
26,-13.0,0
27,-14.3,0
28,-15.6,0
29,-16.9,0
30,-18.2,0
31,-19.5,0
32,-20.8,0
2 changes: 1 addition & 1 deletion pandar_pointcloud/include/pandar_pointcloud/decoder/pandar_xtm.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ constexpr size_t SEQUENCE_SIZE = 4;
constexpr size_t PACKET_TAIL_SIZE = 28;

// All
constexpr size_t PACKET_SIZE = HEAD_SIZE + BODY_SIZE + PACKET_TAIL_SIZE;
constexpr size_t PACKET_SIZE = 820;

// 0x33 - First Return 0x39 - Dual Return (Last, Strongest)
// 0x37 - Strongest Return 0x3B - Dual Return (Last, First)
Expand Down
100 changes: 98 additions & 2 deletions pandar_pointcloud/include/pandar_pointcloud/decoder/pandar_xtm_decoder.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,93 @@ namespace pandar_pointcloud
{
namespace pandar_xtm
{

const float pandarXTM_elev_angle_map[] = {
19.5f, 18.2f, 16.9f, 15.6f, 14.3f, 13.0f, 11.7f, 10.4f, \
9.1f, 7.8f, 6.5f, 5.2f, 3.9f, 2.6f, 1.3f, 0.0f, \
-1.3f, -2.6f, -3.9f, -5.2f, -6.5f, -7.8f, -9.1f, -10.4f, \
-11.7f, -13.0f, -14.3f, -15.6f, -16.9f, -18.2f, -19.5f, -20.8f
};

const float pandarXTM_horizontal_azimuth_offset_map[] = {
0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, \
0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, \
0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, \
0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f
};

const float blockXTMOffsetTriple[] = {
5.632f - 50.0f * 1.0f,
5.632f - 50.0f * 1.0f,
5.632f - 50.0f * 1.0f,
5.632f - 50.0f * 0.0f,
5.632f - 50.0f * 0.0f,
5.632f - 50.0f * 0.0f,
5.632f - 50.0f * 0.0f,
5.632f - 50.0f * 0.0f
};

const float blockXTMOffsetDual[] = {
5.632f - 50.0f * 2.0f,
5.632f - 50.0f * 2.0f,
5.632f - 50.0f * 1.0f,
5.632f - 50.0f * 1.0f,
5.632f - 50.0f * 0.0f,
5.632f - 50.0f * 0.0f,
5.632f - 50.0f * 0.0f,
5.632f - 50.0f * 0.0f
};
const float blockXTMOffsetSingle[] = {
5.632f - 50.0f * 5.0f,
5.632f - 50.0f * 4.0f,
5.632f - 50.0f * 3.0f,
5.632f - 50.0f * 2.0f,
5.632f - 50.0f * 1.0f,
5.632f - 50.0f * 0.0f,
5.632f - 50.0f * 0.0f,
5.632f - 50.0f * 0.0f
};

const float laserXTMOffset[] = {
2.856f * 0.0f + 0.368f,
2.856f * 1.0f + 0.368f,
2.856f * 2.0f + 0.368f,
2.856f * 3.0f + 0.368f,
2.856f * 4.0f + 0.368f,
2.856f * 5.0f + 0.368f,
2.856f * 6.0f + 0.368f,
2.856f * 7.0f + 0.368f,

2.856f * 8.0f + 0.368f,
2.856f * 9.0f + 0.368f,
2.856f * 10.0f + 0.368f,
2.856f * 11.0f + 0.368f,
2.856f * 12.0f + 0.368f,
2.856f * 13.0f + 0.368f,
2.856f * 14.0f + 0.368f,
2.856f * 15.0f + 0.368f,

2.856f * 0.0f + 0.368f,
2.856f * 1.0f + 0.368f,
2.856f * 2.0f + 0.368f,
2.856f * 3.0f + 0.368f,
2.856f * 4.0f + 0.368f,
2.856f * 5.0f + 0.368f,
2.856f * 6.0f + 0.368f,
2.856f * 7.0f + 0.368f,

2.856f * 8.0f + 0.368f,
2.856f * 9.0f + 0.368f,
2.856f * 10.0f + 0.368f,
2.856f * 11.0f + 0.368f,
2.856f * 12.0f + 0.368f,
2.856f * 13.0f + 0.368f,
2.856f * 14.0f + 0.368f,
2.856f * 15.0f + 0.368f
};

const uint16_t MAX_AZIMUTH_DEGREE_NUM=36000;

class PandarXTMDecoder : public PacketDecoder
{
public:
Expand All @@ -29,8 +116,6 @@ class PandarXTMDecoder : public PacketDecoder
private:
bool parsePacket(const pandar_msgs::PandarPacket& raw_packet);
PointcloudXYZIRADT convert(const int block_id);
PointcloudXYZIRADT convert_dual(const int block_id);
PointcloudXYZIRADT convert_triple(const int block_id);

std::array<float, UNIT_NUM> elev_angle_;
std::array<float, UNIT_NUM> azimuth_offset_;
Expand All @@ -40,6 +125,12 @@ class PandarXTMDecoder : public PacketDecoder
std::array<float, BLOCK_NUM> block_offset_dual_;
std::array<float, BLOCK_NUM> block_offset_triple_;

std::vector<float> m_sin_elevation_map_;
std::vector<float> m_cos_elevation_map_;

std::vector<float> m_sin_azimuth_map_;
std::vector<float> m_cos_azimuth_map_;

ReturnMode return_mode_;
Packet packet_;

Expand All @@ -49,6 +140,11 @@ class PandarXTMDecoder : public PacketDecoder
uint16_t scan_phase_;
int last_phase_;
bool has_scanned_;
uint16_t last_azimuth_;
int start_angle_;
double last_timestamp_;

void CalcXTPointXYZIT(int blockid, char chLaserNumber, boost::shared_ptr<pcl::PointCloud<PointXYZIRADT>> cld);
};

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

<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="Strongest"/>
<arg name="dual_return_distance_threshold" default="0.1"/>
<arg name="model" default="PandarXTM"/>
<arg name="frame_id" default="pandar"/>
<arg name="calibration" default="$(find pandar_pointcloud)/config/xtm.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 a565ae7

Please sign in to comment.