ray ground filter does not work
Description
While testing this MR https://gitlab.com/AutowareAuto/AutowareAuto/merge_requests/51 we figured out that the ray ground filter does not work.
For issues see the reproducer below.
How to Reproduce
- term1
udpreplay ~/data/route_small_loop_rw-127.0.0.1.pcap
- term2
ros2 run velodyne_node velodyne_cloud_node_exe --config_file /home/"${USER}"/AutowareAuto/src/drivers/velodyne_node/param/vlp16_test.param.yaml
- term3
ros2 run ray_ground_classifier_nodes ray_ground_classifier_cloud_node_exe --config_file /home/${USER}/AutowareAuto/src/perception/filters/ray_ground_classifier_nodes/param/vlp16_lexus.param.yaml
For the 3rd step we needed to do the following:
This starts the node successfully:
diff --git a/src/perception/filters/ray_ground_classifier_nodes/src/ray_ground_classifier_cloud_node_main.cpp b/src/perception/filters/ray_ground_classifier_nodes/src/ray_ground_classifier_cloud_node_main.cpp
index 1aefd7e..e27a69b 100644
--- a/src/perception/filters/ray_ground_classifier_nodes/src/ray_ground_classifier_cloud_node_main.cpp
+++ b/src/perception/filters/ray_ground_classifier_nodes/src/ray_ground_classifier_cloud_node_main.cpp
@@ -44,6 +44,7 @@ int32_t main(const int32_t argc, char * argv[])
node_namespace = arg;
}
// TODO(christopher.ho) #1270apex_app
+ rclcpp::init(argc, argv);
using
autoware::perception::filters::ray_ground_classifier_nodes::RayGroundClassifierCloudNode;
const auto nd_ptr = std::make_shared<RayGroundClassifierCloudNode>(
This allocates the messages correctly, e.g. we do not get this error:
...
[INFO] [ray_ground_classifier]: RayGroundClassifierNode: Overran nonground msg point capacity
[INFO] [ray_ground_classifier]: RayGroundClassifierNode: Overran ground msg point capacity
...
diff --git a/src/perception/filters/ray_ground_classifier_nodes/src/ray_ground_classifier_cloud_node.cpp b/src/perception/filters/ray_ground_classifier_nodes/src/ray_ground_classifier_cloud_node.cpp
index c1d2595..288a622 100644
--- a/src/perception/filters/ray_ground_classifier_nodes/src/ray_ground_classifier_cloud_node.cpp
+++ b/src/perception/filters/ray_ground_classifier_nodes/src/ray_ground_classifier_cloud_node.cpp
@@ -14,6 +14,7 @@
// limitations under the License.
#include <string>
+#include "sensor_msgs/point_cloud2_iterator.hpp"
#include "lidar_utils/lidar_types.hpp"
#include "lidar_utils/point_cloud_utils.hpp"
#include "ray_ground_classifier_nodes/ray_ground_classifier_cloud_node.hpp"
@@ -207,6 +208,10 @@ void RayGroundClassifierCloudNode::reset()
m_nonground_msg.width = 0U;
m_ground_msg.data.clear();
m_ground_msg.width = 0U;
+ sensor_msgs::PointCloud2Modifier pc_modifier1(m_ground_msg);
+ sensor_msgs::PointCloud2Modifier pc_modifier2(m_nonground_msg);
+ pc_modifier1.resize(m_pcl_size);
+ pc_modifier2.resize(m_pcl_size);
}
////////////////////////////////////////////////////////////////////////////////
void RayGroundClassifierCloudNode::register_callbacks_preallocate()
@@ -226,6 +231,10 @@ void RayGroundClassifierCloudNode::register_callbacks_preallocate()
// initialize messages
init_pcl_msg(m_ground_msg, m_frame_id.c_str(), m_pcl_size);
init_pcl_msg(m_nonground_msg, m_frame_id.c_str(), m_pcl_size);
+ sensor_msgs::PointCloud2Modifier pc_modifier1(m_ground_msg);
+ sensor_msgs::PointCloud2Modifier pc_modifier2(m_nonground_msg);
+ pc_modifier1.resize(m_pcl_size);
+ pc_modifier2.resize(m_pcl_size);
}
////////////////////////////////////////////////////////////////////////////////
void RayGroundClassifierCloudNode::reset_cloud_idx()
This is a HACK and allows to compile after the allocation change above:
diff --git a/src/perception/filters/ray_ground_classifier_nodes/CMakeLists.txt b/src/perception/filters/ray_ground_classifier_nodes/CMakeLists.txt
index 4f12f58..25ea23f 100644
--- a/src/perception/filters/ray_ground_classifier_nodes/CMakeLists.txt
+++ b/src/perception/filters/ray_ground_classifier_nodes/CMakeLists.txt
@@ -35,6 +35,11 @@ ament_target_dependencies(${CLOUD_NODE_EXE})
autoware_set_compile_options(${CLOUD_NODE_EXE})
target_compile_definitions(${CLOUD_NODE_EXE} PRIVATE CMAKE_INSTALL_PREFIX="${CMAKE_INSTALL_PREFIX}")
+# adding conversion error suppresion because of the /opt/ros/dashing/include/sensor_msgs/impl/point_cloud2_iterator.hpp
+# TODO: figure out the right way to supress this in e.g. sensor_msgs
+target_compile_options(${CLOUD_NODE_EXE} PRIVATE -Wno-sign-conversion -Wno-conversion)
+target_compile_options(${CLOUD_NODE_LIB} PRIVATE -Wno-sign-conversion -Wno-conversion)
+
if(BUILD_TESTING)
# run linters
autoware_static_code_analysis()
dejan.pangercic@ade:~/AutowareAuto/
This connects the right topics between the velodyne_node and ray ground filter node:
diff --git a/src/perception/filters/ray_ground_classifier_nodes/param/vlp16_lexus.param.yaml b/src/perception/filters/ray_ground_classifier_nodes/param/vlp16_lexus.param.yaml
index b8d9df0..da2f9c6 100644
--- a/src/perception/filters/ray_ground_classifier_nodes/param/vlp16_lexus.param.yaml
+++ b/src/perception/filters/ray_ground_classifier_nodes/param/vlp16_lexus.param.yaml
@@ -5,7 +5,7 @@ ray_ground_classifier:
ros__parameters:
timeout_ms: 10
cloud_timeout_ms: 110
- raw_topic: "blocks_raw"
+ raw_topic: "test_velodyne_node_cloud_front"
ground_topic: "points_ground"
nonground_topic: "nonground_points"
pcl_size: 55000
Current Behavior
Now the problem is that ground and non-ground are not segmented correctly.
E.g. this is a full point cloud
And this is non ground point cloud which does not have correct number of points