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

Generalized Lidar Simulator PR #78

Merged
merged 6 commits into from
Apr 17, 2020
Merged

Generalized Lidar Simulator PR #78

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
fabddee
add generalized lidar simulator, Closes #74
drehermarc Apr 2, 2020
db8a425
add changes corresponding to feedback, Closes #74
drehermarc Apr 2, 2020
5cb1ea0
Revert "add changes corresponding to feedback, Closes #74"
drehermarc Apr 3, 2020
2a66b33
add changes according to feedback, retry
drehermarc Apr 3, 2020
313a88e
add changes to latest comments
drehermarc Apr 3, 2020
49bee56
merged from master
drehermarc Apr 8, 2020
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
2 changes: 1 addition & 1 deletion cpt_matching_algorithms/config/rviz_movable_lidar_sim.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@ Panels:
Experimental: false
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Name: Time
SyncMode: 0
SyncSource: ""
SyncSource: PointCloud2
Preferences:
PromptSaveOnExit: true
Toolbars:
Expand Down
2 changes: 1 addition & 1 deletion cpt_matching_algorithms/config/sim_lidar_config.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@ groundtruth_orientation: [0.6894984, 0, 0, 0.724872] #w x y z

#LiDAR settings
accuracy_of_lidar: 0.0
range_of_lidar: 50
range_of_lidar: 1000
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
Expand Down
4 changes: 2 additions & 2 deletions cpt_matching_algorithms/config/test_match_config.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,6 @@ mapSamplingDensity: 20
usetoyproblem: true

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

24 changes: 9 additions & 15 deletions cpt_matching_algorithms/launch/movable_lidar_sim.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,42 +4,36 @@
<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" />
<arg name="lidar_frame" value="simulated_lidar_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)" />

<!-- If FixLidarScan is true, the scan does not move at all but stays centered (fixed) at the origin, if FixLidarScan is false, the scan moves with the marker as origin -->
<param name="FixLidarScans" value="false" />
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

<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">
type="position_marker_in_mesh.py"
if ="$(arg SimMarkerLidar)">
<param name="marker_parent_frame" value="map" />
<param name="marker_frame_name" value="$(arg lidar_frame)" />
<param name="marker_name" value="Simulated LiDAR" />
</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"
type="position_cad_with_marker.py">
<param name="marker_parent_frame" value="map" />
<param name="marker_frame_name" value="marker" />
</node>
<node pkg="tf" type="static_transform_publisher" name="SimFixLidar" args="-2 2 1 0 0 0.724872 0.6894984 map $(arg lidar_frame) 100" unless ="$(arg SimMarkerLidar)"/>

<node pkg="rosservice" type="rosservice" name="cad_trigger" args="call /mesh_publisher/publish {}" launch-prefix="bash -c 'sleep 2; $0 $@' "/>
<node pkg="tf" type="static_transform_publisher" name="mesh_positioning" args="0 0 0 0 0 1 1 marker map 100"/>
<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)"/>
<param name="frame_name" value="marker"/>
</node>

<node pkg="tf" type="static_transform_publisher" name="laser" args="100 0 0 0 0 0 1 /lidar /rslidar 100"/>
<node pkg="tf" type="static_transform_publisher" name="laser" args="0 0 0 0 0 0 1 /lidar /rslidar 100"/>

<node name="rviz" pkg="rviz" type="rviz" args="-d $(find cpt_matching_algorithms)/config/rviz_movable_lidar_sim.yaml" />

Expand Down
8 changes: 2 additions & 6 deletions cpt_matching_algorithms/launch/test_matcher.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,12 +31,8 @@

<node name="LiDAR_driver" pkg="rosbag" type="play" args="-l $(arg lidar_scan)" />

<node name="mesh_positioning"
pkg="cpt_selective_icp"
type="position_cad_with_marker.py">
<param name="marker_parent_frame" value="map" />
<param name="marker_frame_name" value="marker" />
</node>
<node pkg="rosservice" type="rosservice" name="cad_trigger" args="call /mesh_publisher/publish {}" launch-prefix="bash -c 'sleep 2; $0 $@' "/>
<node pkg="tf" type="static_transform_publisher" name="mesh_positioning" args="0 0 0 0 0 1 1 marker map 100"/>
hermannsblum marked this conversation as resolved.
Show resolved Hide resolved

<node name="find_id_in_mesh"
pkg="cpt_utils"
Expand Down
11 changes: 5 additions & 6 deletions cpt_matching_algorithms/src/test_matcher/sim_lidar_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,6 @@
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <tf_conversions/tf_eigen.h>
#include <visualization_msgs/InteractiveMarkerFeedback.h>
#include <random>

typedef PointMatcher<float> PM;
Expand All @@ -23,7 +22,7 @@ PointCloud lidar_scan;

bool got_CAD = false;
std::string tf_map_frame;
std::string frame_id;
std::string mesh_frame_id;

bool fix_lidar_scan;

Expand Down Expand Up @@ -58,7 +57,7 @@ int main(int argc, char **argv) {
noise_variance = nh_private.param<float>("accuracy_of_lidar", 0.02);

fix_lidar_scan = nh_private.param<bool>("FixLidarScans", true);
tf_lidar_frame = nh_private.param<std::string>("lidarFrame", "fail");
tf_lidar_frame = nh_private.param<std::string>("lidarFrame", "simulated_lidar_pose");

// Get mesh
std::string cad_topic = nh_private.param<std::string>("cadTopic", "fail");
Expand All @@ -79,15 +78,15 @@ 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;
frame_id = cad_mesh_in.header.frame_id;
mesh_frame_id = cad_mesh_in.header.frame_id;
cad_percept::cgal::msgToMeshModel(cad_mesh_in.mesh, &reference_mesh);

// Get transformation from /map to mesh
tf::StampedTransform transform;
tf::TransformListener tf_listener(ros::Duration(30));
try {
tf_listener.waitForTransform(tf_map_frame, frame_id, ros::Time(0), ros::Duration(5.0));
tf_listener.lookupTransform(tf_map_frame, frame_id, ros::Time(0),
tf_listener.waitForTransform(tf_map_frame, mesh_frame_id, ros::Time(0), ros::Duration(5.0));
tf_listener.lookupTransform(tf_map_frame, mesh_frame_id, ros::Time(0),
transform); // get transformation at latest time T_map_to_frame
} catch (tf::TransformException ex) {
ROS_ERROR_STREAM("Couldn't find transformation to mesh system");
Expand Down
4 changes: 2 additions & 2 deletions cpt_utils/src/position_marker_in_mesh.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,10 +17,10 @@ def frame_callback(marker_msg, tf_broadcaster, map_frame):
time, rospy.get_param('~marker_frame_name'), map_frame)

if __name__ == "__main__":
rospy.init_node("find_id_in_mesh")
rospy.init_node("Marker Pose")
transform_pub = rospy.Publisher('/T_map_marker', Transform, queue_size=10)
tf_broadcaster = TransformBroadcaster()
marker = RosMarker('Marker Pose',
marker = RosMarker(rospy.get_param('~marker_name'),
'Move this marker',
'marker_pose',
InteractiveMarkerControl.MOVE_ROTATE_3D,
Expand Down