Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

add publishing of laser beam visualization markers #509

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

add publishing of laser beam visualization markers #509

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
9f5629c
add publishing of laser beam visualization markers
mantkiew Apr 17, 2023
fe18ad3
move implementation to cpp and simplify code. Publish lidar beams in …
mantkiew Apr 18, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 13 additions & 0 deletions velodyne_pointcloud/include/velodyne_pointcloud/transform.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,9 @@
#include <dynamic_reconfigure/server.h>
#include <velodyne_pointcloud/TransformNodeConfig.h>

#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

namespace velodyne_pointcloud
{
using TransformNodeCfg = velodyne_pointcloud::TransformNodeConfig;
Expand All @@ -76,9 +79,19 @@ class Transform
boost::shared_ptr<dynamic_reconfigure::Server<velodyne_pointcloud::TransformNodeConfig>> srv_;
void reconfigure_callback(velodyne_pointcloud::TransformNodeConfig& config, uint32_t level);

visualization_msgs::Marker create_laser_marker(const std::string & frame_id,
const std::string & ns,
const size_t & marker_id,
const float & marker_color_r,
const float & marker_color_g,
const float & marker_color_b,
const geometry_msgs::Point & pt1,
const geometry_msgs::Point & pt2) const;

boost::shared_ptr<velodyne_rawdata::RawData> data_;
ros::Subscriber velodyne_scan_;
ros::Publisher output_;
ros::Publisher rviz_;

/// configuration parameters
typedef struct
Expand Down
2 changes: 1 addition & 1 deletion velodyne_pointcloud/launch/32e_points.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@
<arg name="calibration" value="$(arg calibration)"/>
<arg name="manager" value="$(arg manager)" />
<arg name="fixed_frame" value="" />
<arg name="target_frame" value="" />
<arg name="target_frame" value="$(arg frame_id)" />
<arg name="max_range" value="$(arg max_range)"/>
<arg name="min_range" value="$(arg min_range)"/>
<arg name="organize_cloud" value="$(arg organize_cloud)"/>
Expand Down
2 changes: 1 addition & 1 deletion velodyne_pointcloud/launch/64e_S3.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@
<arg name="calibration" value="$(arg calibration)"/>
<arg name="manager" value="$(arg manager)" />
<arg name="fixed_frame" value="" />
<arg name="target_frame" value="" />
<arg name="target_frame" value="$(arg frame_id)" />
<arg name="max_range" value="$(arg max_range)"/>
<arg name="min_range" value="$(arg min_range)"/>
</include>
Expand Down
2 changes: 1 addition & 1 deletion velodyne_pointcloud/launch/VLP-32C_points.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@
<arg name="calibration" value="$(arg calibration)"/>
<arg name="manager" value="$(arg manager)" />
<arg name="fixed_frame" value="" />
<arg name="target_frame" value="" />
<arg name="target_frame" value="$(arg frame_id)" />
<arg name="max_range" value="$(arg max_range)"/>
<arg name="min_range" value="$(arg min_range)"/>
<arg name="organize_cloud" value="$(arg organize_cloud)"/>
Expand Down
2 changes: 1 addition & 1 deletion velodyne_pointcloud/launch/VLP16_points.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@
<arg name="calibration" value="$(arg calibration)"/>
<arg name="manager" value="$(arg manager)" />
<arg name="fixed_frame" value="" />
<arg name="target_frame" value="" />
<arg name="target_frame" value="$(arg frame_id)" />
<arg name="max_range" value="$(arg max_range)"/>
<arg name="min_range" value="$(arg min_range)"/>
<arg name="organize_cloud" value="$(arg organize_cloud)"/>
Expand Down
2 changes: 1 addition & 1 deletion velodyne_pointcloud/launch/VLS128_points.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,7 @@
<arg name="calibration" value="$(arg calibration)"/>
<arg name="manager" value="$(arg manager)" />
<arg name="fixed_frame" value="" />
<arg name="target_frame" value="" />
<arg name="target_frame" value="$(arg frame_id)" />
<arg name="max_range" value="$(arg max_range)"/>
<arg name="min_range" value="$(arg min_range)"/>
<arg name="organize_cloud" value="$(arg organize_cloud)"/>
Expand Down
85 changes: 81 additions & 4 deletions velodyne_pointcloud/src/conversions/transform.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,14 +31,61 @@ namespace velodyne_pointcloud
first_rcfg_call(true),
diagnostics_(node, private_nh, node_name)
{
const std::string marker_frame_id{private_nh.param<std::string>("target_frame", "velodyne")};
const std::string marker_ns{private_nh.param<std::string>("model", "64E")};
const float max_range{private_nh.param<float>("max_range", 200.0)};

boost::optional<velodyne_pointcloud::Calibration> calibration = data_->setup(private_nh);
if(calibration)
{

if (calibration) {
ROS_DEBUG_STREAM("Calibration file loaded.");
config_.num_lasers = static_cast<uint16_t>(calibration.get().num_lasers);
// publish RViz markers
visualization_msgs::MarkerArray marker_array;
size_t marker_id{0};
geometry_msgs::Point origin_point;
origin_point.x = origin_point.y = origin_point.z = 0.0;
// white for 128 beam
float marker_color_r{1.0};
float marker_color_g{1.0};
float marker_color_b{1.0};
switch (calibration.get().num_lasers) {
case 64: marker_color_g = 0.0; break;
case 32: marker_color_r = 0.0; break;
case 16: marker_color_b = 0.0; break;
}
geometry_msgs::Point other_point;
other_point.y = 0.0;
for (const auto laser_correction : calibration->laser_corrections) {
// front point
other_point.x = laser_correction.cos_vert_correction * max_range;
other_point.z = laser_correction.sin_vert_correction * max_range;
marker_array.markers.push_back(
create_laser_marker(marker_frame_id,
marker_ns,
marker_id++,
marker_color_r,
marker_color_g,
marker_color_b,
origin_point,
other_point));
// rear point
other_point.x = -other_point.x;
marker_array.markers.push_back(
create_laser_marker(marker_frame_id,
marker_ns,
marker_id++,
marker_color_r,
marker_color_g,
marker_color_b,
origin_point,
other_point));
}
this->rviz_ = private_nh.advertise<visualization_msgs::MarkerArray>(
"lidar_beams", 1, true);
this->rviz_.publish(marker_array);
}
else
{
else {
ROS_ERROR_STREAM("Could not load calibration file!");
}

Expand Down Expand Up @@ -146,4 +193,34 @@ namespace velodyne_pointcloud
diagnostics_.update();
}

visualization_msgs::Marker Transform::create_laser_marker(const std::string & frame_id,
const std::string & ns,
const size_t & marker_id,
const float & marker_color_r,
const float & marker_color_g,
const float & marker_color_b,
const geometry_msgs::Point & pt1,
const geometry_msgs::Point & pt2) const {
visualization_msgs::Marker line_marker;
line_marker.ns = ns;
line_marker.header.frame_id = frame_id;
line_marker.header.stamp = ros::Time(0);
line_marker.id = marker_id;
line_marker.type = visualization_msgs::Marker::LINE_STRIP;
line_marker.action = visualization_msgs::Marker::ADD;
line_marker.pose.position.x = line_marker.pose.position.y = line_marker.pose.position.z = 0.0;
line_marker.pose.orientation.x = line_marker.pose.orientation.y = line_marker.pose.orientation.z = 0.0;
line_marker.pose.orientation.w = 1.0;
line_marker.scale.x = 0.005;
line_marker.color.a = 1;
line_marker.color.r = marker_color_r;
line_marker.color.g = marker_color_g;
line_marker.color.b = marker_color_b;
line_marker.lifetime = ros::Duration(0.0); // lifetime is forever
line_marker.frame_locked = true; // re-update every frame change
line_marker.points.push_back(pt1);
line_marker.points.push_back(pt2);
return line_marker;
}

} // namespace velodyne_pointcloud