laserscan publisher added

This commit is contained in:
Rooholla-KhorramBakht 2024-02-13 19:32:03 -05:00
parent d089d53f5e
commit b535e6595a
1 changed files with 69 additions and 2 deletions

View File

@ -40,6 +40,8 @@
#include <string>
#include <functional>
#include <stdbool.h>
#include <math.h>
#include "sensor_msgs/msg/laser_scan.hpp"
#ifdef __CUDACC__
#include "hesai_lidar_sdk_gpu.cuh"
#else
@ -62,6 +64,9 @@ public:
LidarDecodedFrame<LidarPointXYZIRT> pointcloud_;
void publishPointcloud(void);
bool publish_pointcloud_flag;
void publishLaserScan(
sensor_msgs::msg::PointCloud2 *cloud_msg);
protected:
// Save packets subscribed by 'ros_recv_packet_topic'
void RecievePacket(const hesai_ros_driver::msg::UdpFrame::SharedPtr msg);
@ -82,8 +87,10 @@ protected:
rclcpp::Subscription<hesai_ros_driver::msg::UdpFrame>::SharedPtr pkt_sub_;
rclcpp::Publisher<hesai_ros_driver::msg::UdpFrame>::SharedPtr pkt_pub_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_;
std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::LaserScan>> laserscan_pub_;
//spin thread while recieve data from ROS topic
boost::thread* subscription_spin_thread_;
};
@ -137,7 +144,7 @@ inline void SourceDriver::Init(const YAML::Node& config)
YamlRead<std::string>(config["ros"], "ros_send_packet_topic", ros_send_packet_topic, "hesai_packets");
pkt_pub_ = node_ptr_->create_publisher<hesai_ros_driver::msg::UdpFrame>(ros_send_packet_topic, 1);
}
laserscan_pub_ = node_ptr_->create_publisher<sensor_msgs::msg::LaserScan>("/go2/scan", rclcpp::SensorDataQoS());
if (driver_param.input_param.source_type == DATA_FROM_ROS_PACKET) {
std::string ros_recv_packet_topic;
YamlRead<std::string>(config["ros"], "ros_recv_packet_topic", ros_recv_packet_topic, "hesai_packets");
@ -193,7 +200,9 @@ inline void SourceDriver::SendPointCloud(const LidarDecodedFrame<LidarPointXYZIR
inline void SourceDriver::publishPointcloud(void)
{
pub_->publish(ToRosMsg(pointcloud_, frame_id_));
sensor_msgs::msg::PointCloud2 msg = ToRosMsg(pointcloud_, frame_id_);
pub_->publish(msg);
publishLaserScan(&msg);
}
inline sensor_msgs::msg::PointCloud2 SourceDriver::ToRosMsg(const LidarDecodedFrame<LidarPointXYZIRT>& frame, const std::string& frame_id)
@ -277,4 +286,62 @@ inline void SourceDriver::RecievePacket(const hesai_ros_driver::msg::UdpFrame::S
}
}
inline void SourceDriver::publishLaserScan(
sensor_msgs::msg::PointCloud2 *cloud_msg)
{
// build laserscan output
auto scan_msg = std::make_unique<sensor_msgs::msg::LaserScan>();
scan_msg->header = cloud_msg->header;
scan_msg->angle_min = -3.1415;
scan_msg->angle_max = 3.1415;
scan_msg->angle_increment = 2*3.1415/1800.;
scan_msg->time_increment = 0.0;
scan_msg->scan_time = 1./10.;
scan_msg->range_min = 0.2;
scan_msg->range_max = 100;
float max_height_ = 0.5;
float min_height_ = -0.5;
// determine amount of rays to create
uint32_t ranges_size = std::ceil(
(scan_msg->angle_max - scan_msg->angle_min) / scan_msg->angle_increment);
// determine if laserscan rays with no obstacle data will evaluate to infinity or max_range
scan_msg->ranges.assign(ranges_size, std::numeric_limits<double>::infinity());
// Iterate through pointcloud
for (sensor_msgs::PointCloud2ConstIterator<float> iter_x(*cloud_msg, "x"),
iter_y(*cloud_msg, "y"), iter_z(*cloud_msg, "z");
iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
{
if (std::isnan(*iter_x) || std::isnan(*iter_y) || std::isnan(*iter_z)) {
continue;
}
if (*iter_z > max_height_ || *iter_z < min_height_) {
continue;
}
double range = hypot(*iter_x, *iter_y);
if (range < scan_msg->range_min) {
continue;
}
if (range > scan_msg->range_max) {
continue;
}
double angle = atan2(*iter_y, *iter_x);
if (angle < scan_msg->angle_min || angle > scan_msg->angle_max) {
continue;
}
// overwrite range at laserscan ray if new range is smaller
int index = (angle - scan_msg->angle_min) / scan_msg->angle_increment;
if (range < scan_msg->ranges[index]) {
scan_msg->ranges[index] = range;
}
}
laserscan_pub_->publish(std::move(scan_msg));
}