Generalized Lidar Simulator PR #78

drehermarc · 2020-04-02T09:36:17Z

Hi :)

You can find the commit of the discussed generalized lidar simulator in this PR. The parameter SimMarker in the movable_lidar_sim.launch allows for moving the lidar scan around with the interactive marker if set on true, if set on false it will use the ground truth data in the config file of the simulator. Additionally, I have implemented the option to fix the lidar scan on the cad model. This can be switched on by setting the parameter FixLidarScan on true (also in the movable_lidar_sim.launch file). If set on false, it will create a point cloud of the lidar scan, where the lidar sensor is centered in the origin. Both variables are set per default on false in sim_lidar_node.cpp such that there is no change in the behavior for the already existing files.

Best,
Marc

hermannsblum · 2020-04-02T14:24:23Z

Thanks for the effort!

Unfortunately, the changes made the code more complex instead of more simple. I think there is a much leaner solution :)

Please have a look at the following:

you do not have to listen on the marker topic, this is only for rviz for interaction with the marker.
right now you are reusing the 'find id in mesh' marker, which is not really made for this job. Please create your own marker node. You could do this even in cpt_utils because this would be a good tool in general. The marker should in principle rather follow the design of the mesh-positioning marker and just continuously publish it's position relative to the 'marker' frame (this is a bit confusing, but the base-coordinate-frame of the cad-model is usually called 'marker' because this was the first time we used markers...
There should be parameter change between the lidar simulation and using a constant position. For a constant position, you don't really have to use a config file, you can just use the static transform publisher. Which node to start can be handled purely in roslaunch, so the lidar-simulation node will just have a tf-listener for a specific frame and simulate the lidar at this frame.
I don't fully understand the FixLidarScan option. Isn't this the same thing as using the coordinates (0, 0, 0) as input?

drehermarc · 2020-04-02T14:55:28Z

Thanks for the effort!
Unfortunately, the changes made the code more complex instead of more simple. I think there is a much leaner solution :)
Please have a look at the following:

you do not have to listen on the marker topic, this is only for rviz for interaction with the marker.
right now you are reusing the 'find id in mesh' marker, which is not really made for this job. Please create your own marker node. You could do this even in cpt_utils because this would be a good tool in general. The marker should in principle rather follow the design of the mesh-positioning marker and just continuously publish it's position relative to the 'marker' frame (this is a bit confusing, but the base-coordinate-frame of the cad-model is usually called 'marker' because this was the first time we used markers...
There should be parameter change between the lidar simulation and using a constant position. For a constant position, you don't really have to use a config file, you can just use the static transform publisher. Which node to start can be handled purely in roslaunch, so the lidar-simulation node will just have a tf-listener for a specific frame and simulate the lidar at this frame.
I don't fully understand the FixLidarScan option. Isn't this the same thing as using the coordinates (0, 0, 0) as input?

Hi Hermann :)
Thanks for your feedback.
I will change the code according to the first three points and try to make the code simpler. To come back to your question from the last point, FixLidarScan is just a visualization tool that does not move the sampled point cloud but the viewing point of the lidar and then creates a lidar frame. This has the advantage that in the visualization the simulated lidar frame is placed exactly on the mesh, but the lidar is no longer central in the point cloud. If FixLidarScan = false, the lidar is again in the center of the point cloud, as it was. Generally you can say that FixLidarScan moves the Point Cloud in its own coordinate system (true) or not (false). I will also try to make this easier.

This reverts commit db8a425.

gawela

Thank you for the contribution. I would try to make it a bit more generic by just providing a pose / transformation message topic (whichever source it is, e.g., marker, ground truth message etc) and then publish the simulated lidar from there. For the static offset you would then simply add this to this transformation (or to zero, if this published topic does not exist). Ideally you would then use much less parameters.and special cases.

gawela · 2020-04-03T06:58:17Z

cpt_matching_algorithms/launch/movable_lidar_sim.launch

+        <param name="SimMarker" value="true" />
+        <param name="FixLidarScans" value="true" />
+        <param name="SimMarkerPoseTopic" value="/mesh_id_marker/feedback" />
+        <param name="cadTopic" value="/mesh_publisher/mesh_out" />


I think it would be enough to just give a pose or transformation topic this node subscribes to this can then for instance come from an interactive marker. Then you don't need a specific marker flag.

gawela · 2020-04-03T06:59:16Z

cpt_matching_algorithms/src/test_matcher/sim_lidar_node.cpp

@@ -23,6 +24,9 @@ PointCloud lidar_scan;
 bool got_CAD = false;
 std::string tf_map_frame;

+bool use_marker_for_sim;


As mentioned above, ideally not work with an extra flag for the marker, only with a subscribed topic.

gawela · 2020-04-03T07:01:30Z

cpt_matching_algorithms/src/test_matcher/sim_lidar_node.cpp

@@ -38,6 +42,7 @@ float noise_variance;
 ros::Publisher scan_pub;

 void getCAD(const cgal_msgs::TriangleMeshStamped &cad_mesh_in);
+void sim_lidar_at_marker(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);


I would integrate this function with the regular simulate_lidar() function.

gawela · 2020-04-03T07:08:31Z

cpt_matching_algorithms/src/test_matcher/sim_lidar_node.cpp

+  use_marker_for_sim = nh_private.param<bool>("SimMarker", false);
+  fix_lidar_scan = nh_private.param<bool>("FixLidarScans", false);
+  ros::Subscriber marker_pos_sub;
+  if (use_marker_for_sim) {
+    std::cout << "Move marker to get lidar scans" << std::endl;
+    std::string marker_pos_topic = nh_private.param<std::string>("SimMarkerPoseTopic", "fail");
+    marker_pos_sub = nh.subscribe(marker_pos_topic, 20, &sim_lidar_at_marker);
+    // Push back no rotation
+    quat.push_back(1);
+    quat.push_back(0);
+    quat.push_back(0);
+    quat.push_back(0);
+


THis should be more generic: e.g., checking if a pose / transformation topic is given as a parameter and if so listening to. Otherwise revert to a standard behaviour (e.g., publishing from the origin).

gawela · 2020-04-03T07:09:13Z

cpt_matching_algorithms/src/test_matcher/sim_lidar_node.cpp

+    x = nh_private.param<float>("groundtruthx", 0);
+    y = nh_private.param<float>("groundtruthy", 0);
+    z = nh_private.param<float>("groundtruthz", 0);
+    quat = nh_private.param<std::vector<float>>("groundtruth_orientation", {});


Ok, this would then be the standard behaviour.

gawela · 2020-04-03T07:12:57Z

cpt_matching_algorithms/src/test_matcher/sim_lidar_node.cpp

+void sim_lidar_at_marker(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) {
+  // Change position of lidar to the one of the marker (different coordinate system)
+  x = -(feedback->pose.position.y) - 1;
+  y = feedback->pose.position.x + 1;
+  z = feedback->pose.position.z;
+
+  std::cout << std::endl;
+  std::cout << "Position of marker/lidar: " << x << " " << y << " " << z << std::endl;
+  std::cout << std::endl;
+
+  if (got_CAD) {
    simulate_lidar();
+    lidar_scan.clear();


THis could be fully integrated with simulate_lidar()

gawela · 2020-04-03T07:16:41Z

cpt_matching_algorithms/src/test_matcher/sim_lidar_node.cpp

-  reference_mesh->transform(ctransformation);
-
-  std::cout << "Lidar frame transformed according to ground truth data" << std::endl;
+  if (!fix_lidar_scan) {


Use this parameter as mentioned above and derive it from the input topic (or not).

gawela · 2020-04-03T07:24:46Z

Oh, I see that Hermann has basically commented the same. I guess I hadn't reloaded the github page.

drehermarc · 2020-04-03T07:30:31Z

Oh, I see that Hermann has basically commented the same. I guess I hadn't reloaded the github page.

Hi :), thanks for the feedback. I have uploaded a new version yesterday, the commit is on the generalized_lidar-feature branch but oddly enough does not seem to appear here in the PR. I have now reverted the changes and will try it again, otherwise I will open a new PR.

drehermarc · 2020-04-03T07:54:39Z

cpt_matching_algorithms/config/rviz_movable_lidar_sim.yaml

@@ -30,7 +30,7 @@ Panels:
    Experimental: false


This is the revised version, sorry for the inconvenience. SimLidar now uses the tf transformation between map (due to visualization reasons, switching to "marker" should be possible with a constant transformation in SimLidar) and marker_pose (frame from the interactive marker). I tried to simplify the code of SimLidar and to regulate certain things already in the launch file (constant pose, etc.) to become more modular.

hermannsblum

Wow, you were quick with applying all these changes :)
Looks very good, just have some minor comments about naming etc.

hermannsblum · 2020-04-03T08:47:13Z

cpt_matching_algorithms/config/test_match_config.yaml

@@ -6,6 +6,6 @@ mapSamplingDensity: 20
 usetoyproblem: true

 #Select Matcher (only one)
-use_template: false
-use_go_icp: true
+usetemplate: true


this file should not change in this PR.

hermannsblum · 2020-04-03T08:48:34Z

cpt_matching_algorithms/launch/movable_lidar_sim.launch

+        <param name="lidarFrame" value="$(arg lidar_frame)" />
+    </node>
+
+    <group if ="$(arg SimMarkerLidar)">


I guess this is copied from some of my launch files. In the meantime, I found out that you can add the if="$(arg SimMarkerLidar)" simply to the node itself, you don't really need the group tag around it.

hermannsblum · 2020-04-03T08:49:56Z

cpt_matching_algorithms/launch/movable_lidar_sim.launch

+	<arg name="cad_file" default="$(find cpt_selective_icp)/resources/cla.json" />
+	<arg name="lidar_settings" default="$(find cpt_matching_algorithms)/config/sim_lidar_config.yaml" />
+    <arg name="SimMarkerLidar" value="true" />
+    <arg name="lidar_frame" value="marker_pose" />


maybe a better name for the frame would be simulated_lidar_pose

hermannsblum · 2020-04-03T08:51:17Z

cpt_matching_algorithms/launch/movable_lidar_sim.launch

+        <node pkg="tf" type="static_transform_publisher" name="SimFixLidar" args="-2 2 1 0 0 0.724872 0.6894984 map $(arg lidar_frame)  100"/>
+    </group>
+
+    <node name="mesh_positioning"


Is this node even necessary? I guess a static transform would be sufficient, together with publish_on_start in the mesh_publisher.

I am not really sure about that. Is there a reason why two different fixed frames (map & marker) with different axes are used?

You are right, this is a bit confusing. We needed "map" and "marker" so far because we did not have global localization. The workflow is as follows:

"map" is the frame for the map of the icp localisation. Ie cpt_selective_icp will load a mesh into the map frame and localize the lidar in it. This is very dependent on where the map frame is initialized with respect to the robot.
For this, we use the marker to move the mesh around (this is the mesh published in marker frame). Once we get a good alignment, we call a service and cpt_selective_icp will set map := marker to initialize.
So basically the map frame is only necessary when using cpt_selective_icp. You don't need it here.

hermannsblum · 2020-04-03T08:51:52Z

cpt_matching_algorithms/src/test_matcher/sim_lidar_node.cpp

@@ -11,6 +11,7 @@
 #include <ros/ros.h>
 #include <tf/transform_listener.h>
 #include <tf_conversions/tf_eigen.h>
+#include <visualization_msgs/InteractiveMarkerFeedback.h>


This should not be necessary.

hermannsblum · 2020-04-03T08:53:22Z

cpt_matching_algorithms/src/test_matcher/sim_lidar_node.cpp

  bin_elevation = nh_private.param<std::vector<float>>("bin_elevation", {0});
  dtheta = nh_private.param<float>("lidar_angular_resolution", 1);
  lidar_offset = nh_private.param<float>("lidar_offset", 1.5);
  noise_variance = nh_private.param<float>("accuracy_of_lidar", 0.02);

-  scan_pub = nh.advertise<sensor_msgs::PointCloud2>("sim_rslidar_points", 1, true);
+  fix_lidar_scan = nh_private.param<bool>("FixLidarScans", true);
+  tf_lidar_frame = nh_private.param<std::string>("lidarFrame", "fail");


If there is no sensible default available, please rather check whether the parameter is available and otherwise raise an Error with a good message of what is failing.

hermannsblum · 2020-04-03T08:54:12Z

cpt_matching_algorithms/src/test_matcher/sim_lidar_node.cpp

@@ -79,7 +79,7 @@ int main(int argc, char **argv) {
 void getCAD(const cgal_msgs::TriangleMeshStamped &cad_mesh_in) {
  if (!got_CAD) {
    std::cout << "Processing CAD mesh" << std::endl;
-    std::string frame_id = cad_mesh_in.header.frame_id;
+    frame_id = cad_mesh_in.header.frame_id;


To avoid confusion with the lidar frame, please rename frame_id into something more explicit, like mesh_frame_id.

hermannsblum · 2020-04-03T08:57:28Z

cpt_utils/src/position_marker_in_mesh.py

+    rospy.init_node("find_id_in_mesh")
+    transform_pub = rospy.Publisher('/T_map_marker', Transform, queue_size=10)
+    tf_broadcaster = TransformBroadcaster()
+    marker = RosMarker('Marker Pose',


Maybe you can make these strings parameters, such that e.g. for your usecase, the label would be "Simulated LiDAR", but others could apply useful labels for their applications.

cpt_matching_algorithms/config/sim_lidar_config.yaml

hermannsblum · 2020-04-03T09:00:24Z

cpt_matching_algorithms/launch/movable_lidar_sim.launch

+    <node name="lidar_simulator"  pkg="cpt_matching_algorithms" type="sim_lidar_node" output="screen" launch-prefix="tmux split-window">
+        <rosparam command="load" file="$(arg lidar_settings)" />
+
+        <param name="FixLidarScans" value="false" />


Please add a comment here that explains this parameter better. Usually, the launch-file is what people who just want to use your code will look into first, because it gives a high-level interface to your nodes. So it is important to add explanations to parameters that are not straightforward to understand.

cpt_matching_algorithms/launch/test_matcher.launch

gawela

Yup, this looks good! :)

Generalized Lidar Simulator PR

add generalized lidar simulator, Closes #74

fabddee

drehermarc requested review from gawela and hermannsblum April 2, 2020 09:47

drehermarc added 2 commits April 2, 2020 22:40

add changes corresponding to feedback, Closes #74

db8a425

Revert "add changes corresponding to feedback, Closes #74"

5cb1ea0

This reverts commit db8a425.

gawela suggested changes Apr 3, 2020

View reviewed changes

add changes according to feedback, retry

2a66b33

drehermarc commented Apr 3, 2020

View reviewed changes

hermannsblum approved these changes Apr 3, 2020

View reviewed changes

add changes to latest comments

313a88e

drehermarc commented Apr 3, 2020

View reviewed changes

cpt_matching_algorithms/launch/test_matcher.launch Show resolved Hide resolved

drehermarc mentioned this pull request Apr 8, 2020

Plane Extraction with Filter PR #77

Merged

merged from master

49bee56

drehermarc requested a review from gawela April 9, 2020 11:36

gawela approved these changes Apr 17, 2020

View reviewed changes

drehermarc merged commit 46fe922 into master Apr 17, 2020

drehermarc added a commit that referenced this pull request Sep 8, 2020

Merge pull request #78 from ethz-asl/generalized_lidar-feature

256cfb3

Generalized Lidar Simulator PR

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Generalized Lidar Simulator PR #78

Generalized Lidar Simulator PR #78

drehermarc commented Apr 2, 2020 •

edited

Loading

hermannsblum commented Apr 2, 2020

drehermarc commented Apr 2, 2020

gawela left a comment

gawela Apr 3, 2020

gawela Apr 3, 2020

gawela Apr 3, 2020

gawela Apr 3, 2020

gawela Apr 3, 2020

gawela Apr 3, 2020

gawela Apr 3, 2020

gawela commented Apr 3, 2020

drehermarc commented Apr 3, 2020

drehermarc Apr 3, 2020

hermannsblum left a comment

hermannsblum Apr 3, 2020

hermannsblum Apr 3, 2020

hermannsblum Apr 3, 2020

hermannsblum Apr 3, 2020

drehermarc Apr 3, 2020 •

edited

Loading

hermannsblum Apr 3, 2020

hermannsblum Apr 3, 2020

hermannsblum Apr 3, 2020

hermannsblum Apr 3, 2020

hermannsblum Apr 3, 2020

hermannsblum Apr 3, 2020

gawela left a comment

Generalized Lidar Simulator PR #78

Generalized Lidar Simulator PR #78

Conversation

drehermarc commented Apr 2, 2020 • edited Loading

hermannsblum commented Apr 2, 2020

drehermarc commented Apr 2, 2020

gawela left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

gawela commented Apr 3, 2020

drehermarc commented Apr 3, 2020

Choose a reason for hiding this comment

hermannsblum left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

drehermarc Apr 3, 2020 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

gawela left a comment

Choose a reason for hiding this comment

drehermarc commented Apr 2, 2020 •

edited

Loading

drehermarc Apr 3, 2020 •

edited

Loading