add changes corresponding to feedback, Closes #74

cpt_matching_algorithms/config/rviz_movable_lidar_sim.yaml

@@ -30,7 +30,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: PointCloud2
+    SyncSource: ""
 Preferences:
   PromptSaveOnExit: true
 Toolbars:
@@ -103,7 +103,7 @@ Visualization Manager:
       Show Axes: false
       Show Descriptions: true
       Show Visual Aids: false
-      Update Topic: /mesh_id_marker/update
+      Update Topic: /marker_pose/update
       Value: true
   Enabled: true
   Global Options:
@@ -133,7 +133,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 35.78120040893555
+      Distance: 27.708961486816406
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1

cpt_matching_algorithms/config/sim_lidar_config.yaml

@@ -4,9 +4,8 @@ groundtruthz: 0
 groundtruth_orientation: [0.6894984, 0, 0, 0.724872] #w x y z
 
 #LiDAR settings
-usebins: true
 accuracy_of_lidar: 0.0
-range_of_lidar: 20
+range_of_lidar: 50
 bin_elevation: [-15, -13, -11, -9, -7, -5, -3, -1, 15, 13, 11, 9, 7, 5, 3, 1]
 lidar_angular_resolution: 2
 lidar_offset: 1.5 

cpt_matching_algorithms/config/test_match_config.yaml

@@ -6,6 +6,6 @@ mapSamplingDensity: 20
 usetoyproblem: true
 
 #Select Matcher (only one)
-use_template: true
+usetemplate: true
 use_go_icp: false
 

cpt_matching_algorithms/launch/movable_lidar_sim.launch

@@ -3,15 +3,28 @@
 	<!-- declare arg to be passed in -->
 	<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" />
 
     <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="SimMarker" value="true" />
-        <param name="FixLidarScans" value="true" />
-        <param name="SimMarkerPoseTopic" value="/mesh_id_marker/feedback" />
+
+        <param name="FixLidarScans" value="false" />
         <param name="cadTopic" value="/mesh_publisher/mesh_out" />
+        <param name="lidarFrame" value="$(arg lidar_frame)" />
+    </node>
+
+    <group if ="$(arg SimMarkerLidar)">
+    <node name="marker_pose"
+          pkg="cpt_utils"
+          type="position_marker_in_mesh.py">
+          <param name="marker_parent_frame" value="map" />
+          <param name="marker_frame_name" value="$(arg lidar_frame)" />
     </node>
+    </group>
+    <group unless ="$(arg SimMarkerLidar)">
+        <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"
           pkg="cpt_selective_icp"
@@ -20,12 +33,6 @@
         <param name="marker_frame_name" value="marker" />
     </node>
 
-    <node name="find_id_in_mesh"
-          pkg="cpt_utils"
-          type="find_closest_id_in_mesh.py">
-        <param name="marker_parent_frame" value="marker" />
-    </node>
-
     <node name="mesh_publisher" pkg="cpt_utils" type="mesh_publisher_node">
         <param name="publish_on_start" value="false"/>
         <param name="default_filename" value="$(arg cad_file)"/>

cpt_matching_algorithms/launch/test_matcher.launch

@@ -16,13 +16,18 @@
         <param name="cadTopic" value="/mesh_publisher/mesh_out" />
         <param name="lidarTopic" value="rslidar_points" />
         <param name="simlidarTopic" value="sim_rslidar_points" />
-        <param name="groundTruthTopic" value="ground_truth" />
+        <param name="groundtruthTopic" value="ground_truth" />
     </node>
 
     <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="lidarFrame" value="marker_pose" />
         <param name="cadTopic" value="/mesh_publisher/mesh_out" />
     </node>
+
+    <arg name="lidar_frame" value="marker_pose" />
+    <node pkg="tf" type="static_transform_publisher" name="SimFixLidar" args="-2 2 1 0 0 0.724872 0.6894984 map $(arg lidar_frame)  100"/>
 
     <node name="LiDAR_driver" pkg="rosbag" type="play" args="-l $(arg lidar_scan)" />
 

cpt_matching_algorithms/src/test_matcher/sim_lidar_node.cpp

@@ -23,26 +23,21 @@ PointCloud lidar_scan;
 
 bool got_CAD = false;
 std::string tf_map_frame;
+std::string frame_id;
 
-bool use_marker_for_sim;
 bool fix_lidar_scan;
 
-float x;
-float y;
-float z;
-std::vector<float> quat;
-
 float range_of_lidar;
-bool use_bins;
 std::vector<float> bin_elevation;
 float dtheta;
 float lidar_offset;
 float noise_variance;
 
 ros::Publisher scan_pub;
+std::string tf_lidar_frame;
 
 void getCAD(const cgal_msgs::TriangleMeshStamped &cad_mesh_in);
-void sim_lidar_at_marker(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
+void set_lidar_in_mesh(const ros::TimerEvent &);
 void simulate_lidar();
 
 int main(int argc, char **argv) {
@@ -57,31 +52,13 @@ int main(int argc, char **argv) {
 
   // Parameter from server
   range_of_lidar = nh_private.param<float>("range_of_lidar", 20);
-  use_bins = nh_private.param<bool>("usebins", false);
   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);
 
-  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);
-
-  } else {
-    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", {});
-  }
+  fix_lidar_scan = nh_private.param<bool>("FixLidarScans", true);
+  tf_lidar_frame = nh_private.param<std::string>("lidarFrame", "fail");
 
   // Get mesh
   std::string cad_topic = nh_private.param<std::string>("cadTopic", "fail");
@@ -90,6 +67,9 @@ int main(int argc, char **argv) {
 
   scan_pub = nh.advertise<sensor_msgs::PointCloud2>("sim_rslidar_points", 1, true);
 
+  // Create timer for simulation
+  ros::Timer simtimer = nh.createTimer(ros::Duration(0.01), set_lidar_in_mesh);
+
   ros::spin();
 
   return 0;
@@ -99,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;
     cad_percept::cgal::msgToMeshModel(cad_mesh_in.mesh, &reference_mesh);
 
     // Get transformation from /map to mesh
@@ -120,84 +100,70 @@ void getCAD(const cgal_msgs::TriangleMeshStamped &cad_mesh_in) {
 
     std::cout << "CAD ready" << std::endl;
     got_CAD = true;
-
-    if (!use_marker_for_sim) {
-      simulate_lidar();
-    }
   }
 }
 
-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;
+void set_lidar_in_mesh(const ros::TimerEvent &) {
+  if (got_CAD) {
+    // Get tf transformation from cad to lidar
+    tf::StampedTransform transform;
+    tf::TransformListener tf_listener(ros::Duration(10));
+    try {
+      tf_listener.waitForTransform(tf_map_frame, tf_lidar_frame, ros::Time(0), ros::Duration(5.0));
+      tf_listener.lookupTransform(tf_map_frame, tf_lidar_frame, ros::Time(0), transform);
+    } catch (tf::TransformException ex) {
+      ROS_INFO_STREAM("Couldn't find transformation to lidar frame");
+      return;
+    }
+
+    // Transform Lidar accordingly
+    cad_percept::cgal::Transformation ctransformation;
+    cad_percept::cgal::tfTransformationToCGALTransformation(transform, ctransformation);
+    reference_mesh->transform(ctransformation.inverse());
 
-  std::cout << std::endl;
-  std::cout << "Position of marker/lidar: " << x << " " << y << " " << z << std::endl;
-  std::cout << std::endl;
+    std::cout << std::endl;
+    std::cout << "Position of Lidar: x: " << ctransformation.m(0, 3)
+              << " y: " << ctransformation.m(1, 3) << " z: " << ctransformation.m(2, 3)
+              << std::endl;
+    std::cout << std::endl;
 
-  if (got_CAD) {
+    // Simulate Lidar
     simulate_lidar();
-    lidar_scan.clear();
+
+    // Transform mesh back for next iteration
+    reference_mesh->transform(ctransformation);
   }
 }
 
 void simulate_lidar() {
-  cad_percept::cgal::Point origin;
-  cad_percept::cgal::Transformation ctransformation;
-  if (!fix_lidar_scan) {
-    // Transform point cloud according to ground truth
-    Eigen::Matrix4d transform = Eigen::Matrix4d::Identity();
-    Eigen::Vector3d translation(x, y, z);
-    Eigen::Quaterniond q(quat[0], quat[1], quat[2], quat[3]);
-    transform.block(0, 0, 3, 3) = q.matrix();
-    transform.block(0, 3, 3, 1) = translation;
-    cad_percept::cgal::eigenTransformationToCgalTransformation(transform, &ctransformation);
-    reference_mesh->transform(ctransformation.inverse());
-
-    if (use_marker_for_sim) {
-      origin = cad_percept::cgal::Point(0, 0, 0);
-    } else {
-      origin = cad_percept::cgal::Point(0, 0, lidar_offset);
-    }
-  } else {
-    if (use_marker_for_sim) {
-      origin = cad_percept::cgal::Point(x, y, z);
-    } else {
-      origin = cad_percept::cgal::Point(x, y, z + lidar_offset);
-    }
-  }
-
   // Add lidar properties
   // Bin characteristic & visible
-  if (use_bins) {
-    std::cout << "Simulate bins of LiDAR" << std::endl;
-
-    float x_unit;
-    float y_unit;
-    float z_unit;
-    float PI_angle = (float)(M_PI / 180);
-    cad_percept::cgal::Ray bin_ray;
-    cad_percept::cgal::Point unit_dir;
-    cad_percept::cgal::Intersection inter_point;
-    pcl::PointXYZ pcl_inter_point;
-    lidar_scan.clear();
-    for (auto &bin : bin_elevation) {
-      for (float theta = 0; theta < 360; theta += dtheta) {
-        x_unit = cos(bin * PI_angle) * cos(theta * PI_angle) + origin.x();
-        y_unit = cos(bin * PI_angle) * sin(theta * PI_angle) + origin.y();
-        z_unit = sin(bin * PI_angle) + origin.z();
-        unit_dir = cad_percept::cgal::Point(x_unit, y_unit, z_unit);
-        bin_ray = cad_percept::cgal::Ray(origin, unit_dir);
-
-        if (reference_mesh->isIntersection(bin_ray)) {
-          inter_point = reference_mesh->getIntersection(bin_ray);
-          pcl_inter_point.x = (float)inter_point.intersected_point.x();
-          pcl_inter_point.y = (float)inter_point.intersected_point.y();
-          pcl_inter_point.z = (float)inter_point.intersected_point.z();
-          lidar_scan.push_back(pcl_inter_point);
-        }
+  std::cout << "Simulate bins of LiDAR" << std::endl;
+
+  float x_unit;
+  float y_unit;
+  float z_unit;
+  float PI_angle = (float)(M_PI / 180);
+  cad_percept::cgal::Ray bin_ray;
+  cad_percept::cgal::Point origin(0, 0, 0);
+  cad_percept::cgal::Point unit_dir;
+  cad_percept::cgal::Intersection inter_point;
+  pcl::PointXYZ pcl_inter_point;
+  lidar_scan.clear();
+  for (auto &bin : bin_elevation) {
+    for (float theta = 0; theta < 360; theta += dtheta) {
+      x_unit = cos(bin * PI_angle) * cos(theta * PI_angle);
+      y_unit = cos(bin * PI_angle) * sin(theta * PI_angle);
+      z_unit = sin(bin * PI_angle);
+      unit_dir = cad_percept::cgal::Point(x_unit, y_unit, z_unit);
+      bin_ray = cad_percept::cgal::Ray(origin, unit_dir);
+
+      if (reference_mesh->isIntersection(bin_ray)) {
+        inter_point = reference_mesh->getIntersection(bin_ray);
+        pcl_inter_point.x = (float)inter_point.intersected_point.x();
+        pcl_inter_point.y = (float)inter_point.intersected_point.y();
+        pcl_inter_point.z = (float)inter_point.intersected_point.z();
+        lidar_scan.push_back(pcl_inter_point);
       }
     }
   }
@@ -221,12 +187,12 @@ void simulate_lidar() {
 
   // Publish simulated lidar frame
   DP ref_scan = cad_percept::cpt_utils::pointCloudToDP(lidar_scan);
-  scan_pub.publish(
-      PointMatcher_ros::pointMatcherCloudToRosMsg<float>(ref_scan, tf_map_frame, ros::Time::now()));
-  std::cout << "LiDAR Simulator starts to publish" << std::endl;
-
-  if (!fix_lidar_scan) {
-    // Transform mesh back for next iteration
-    reference_mesh->transform(ctransformation);
+  if (fix_lidar_scan) {
+    scan_pub.publish(PointMatcher_ros::pointMatcherCloudToRosMsg<float>(ref_scan, tf_map_frame,
+                                                                        ros::Time::now()));
+  } else {
+    scan_pub.publish(PointMatcher_ros::pointMatcherCloudToRosMsg<float>(ref_scan, tf_lidar_frame,
+                                                                        ros::Time::now()));
   }
+  std::cout << "LiDAR Simulator starts to publish" << std::endl;
 }

cpt_utils/src/position_marker_in_mesh.py

@@ -0,0 +1,36 @@
+#!/usr/bin/env python
+import rospy
+from geometry_msgs.msg import Transform, Vector3, Quaternion, Point
+from visualization_msgs.msg import InteractiveMarkerControl
+from tf.broadcaster import TransformBroadcaster
+from std_srvs.srv import Empty, EmptyRequest
+
+from cpt_utils.marker import RosMarker
+
+def frame_callback(marker_msg, tf_broadcaster, map_frame):
+    time = rospy.Time.now()
+    pose = marker_msg.pose
+    tf_broadcaster.sendTransform(
+        (pose.position.x, pose.position.y, pose.position.z),
+        (pose.orientation.x, pose.orientation.y, pose.orientation.z,
+         pose.orientation.w),
+        time, rospy.get_param('~marker_frame_name'), map_frame)
+
+if __name__ == "__main__":
+    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',
+                       'Move this marker',
+                       'marker_pose',
+                       InteractiveMarkerControl.MOVE_ROTATE_3D,
+                       show_controls=True,
+                       position=[0, 0, 0],
+                       orientation=[0, 0, 0, 1])
+
+    # Create a timer to regularly query for new IDs
+    rospy.Timer(rospy.Duration(0.01), lambda msg: frame_callback(
+        marker.marker, tf_broadcaster, rospy.get_param('~marker_parent_frame')))
+
+    rospy.spin()
+