laserscan publisher added
This commit is contained in:
parent
d089d53f5e
commit
b535e6595a
|
@ -40,6 +40,8 @@
|
||||||
#include <string>
|
#include <string>
|
||||||
#include <functional>
|
#include <functional>
|
||||||
#include <stdbool.h>
|
#include <stdbool.h>
|
||||||
|
#include <math.h>
|
||||||
|
#include "sensor_msgs/msg/laser_scan.hpp"
|
||||||
#ifdef __CUDACC__
|
#ifdef __CUDACC__
|
||||||
#include "hesai_lidar_sdk_gpu.cuh"
|
#include "hesai_lidar_sdk_gpu.cuh"
|
||||||
#else
|
#else
|
||||||
|
@ -62,6 +64,9 @@ public:
|
||||||
LidarDecodedFrame<LidarPointXYZIRT> pointcloud_;
|
LidarDecodedFrame<LidarPointXYZIRT> pointcloud_;
|
||||||
void publishPointcloud(void);
|
void publishPointcloud(void);
|
||||||
bool publish_pointcloud_flag;
|
bool publish_pointcloud_flag;
|
||||||
|
void publishLaserScan(
|
||||||
|
sensor_msgs::msg::PointCloud2 *cloud_msg);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
// Save packets subscribed by 'ros_recv_packet_topic'
|
// Save packets subscribed by 'ros_recv_packet_topic'
|
||||||
void RecievePacket(const hesai_ros_driver::msg::UdpFrame::SharedPtr msg);
|
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::Subscription<hesai_ros_driver::msg::UdpFrame>::SharedPtr pkt_sub_;
|
||||||
rclcpp::Publisher<hesai_ros_driver::msg::UdpFrame>::SharedPtr pkt_pub_;
|
rclcpp::Publisher<hesai_ros_driver::msg::UdpFrame>::SharedPtr pkt_pub_;
|
||||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr 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
|
//spin thread while recieve data from ROS topic
|
||||||
boost::thread* subscription_spin_thread_;
|
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");
|
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);
|
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) {
|
if (driver_param.input_param.source_type == DATA_FROM_ROS_PACKET) {
|
||||||
std::string ros_recv_packet_topic;
|
std::string ros_recv_packet_topic;
|
||||||
YamlRead<std::string>(config["ros"], "ros_recv_packet_topic", ros_recv_packet_topic, "hesai_packets");
|
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)
|
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)
|
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));
|
||||||
|
}
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue