Not able to get segmented output

[yash4242]

Hello,
I pulled the image, ran it in a container. did a docker cp to take the virtual dataset bagfile into the container.
The bagfile is placed in /root/s_graphs_ws/src/s_graphs/dataset/ path inside the container. I mkdir'd the dataset folder, and docker cp'd the bagfile inside of it.

Then exec -it bash in a few terminal windows to enter the container from multiple terminals. On one of them, I run roslaunch s_graphs s_graphs.launch env:=virtual use_free_space_graph:=true 2>/dev/null in /root/s_graphs_ws/src/s_graphs directory. This gives the following output:

root@orion:~/s_graphs_ws/src/s_graphs# roslaunch s_graphs s_graphs.launch env:=virtual use_free_space_graph:=true 2>/dev/null
... logging to /root/.ros/log/9cb40064-f0a6-11ed-a108-50ebf6bde39a/roslaunch-orion.rrcx.tk-62292.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://orion.rrcx.tk:43269/

SUMMARY
========

CLEAR PARAMETERS
 * /voxblox_skeletonizer/

PARAMETERS
 * /hdl_prefilter/base_link_frame: base_footprint
 * /hdl_prefilter/distance_far_thresh: 100.0
 * /hdl_prefilter/distance_near_thresh: 0.5
 * /hdl_prefilter/downsample_method: VOXELGRID
 * /hdl_prefilter/downsample_resolution: 0.1
 * /hdl_prefilter/outlier_removal_method: RADIUS
 * /hdl_prefilter/radius_min_neighbors: 0.5
 * /hdl_prefilter/radius_radius: 2
 * /hdl_prefilter/statistical_mean_k: 30
 * /hdl_prefilter/statistical_stddev: 1.2
 * /hdl_prefilter/use_distance_filter: True
 * /map2odom_publisher/map_frame_id: map
 * /map2odom_publisher/odom_frame_id: odom
 * /rosdistro: noetic
 * /rosversion: 1.15.14
 * /s_graphs/accum_distance_thresh: 3.0
 * /s_graphs/const_stddev_q: 0.1
 * /s_graphs/const_stddev_x: 0.5
 * /s_graphs/constant_covariance: True
 * /s_graphs/distance_thresh: 1.0
 * /s_graphs/enable_gps: False
 * /s_graphs/enable_imu_acceleration: False
 * /s_graphs/enable_imu_orientation: False
 * /s_graphs/extract_planar_surfaces: True
 * /s_graphs/fitness_score_thresh: 0.5
 * /s_graphs/fix_first_node: True
 * /s_graphs/fix_first_node_adaptive: True
 * /s_graphs/fix_first_node_stddev: 1 1 1 1 1 1
 * /s_graphs/floor_edge_robust_kernel: NONE
 * /s_graphs/floor_edge_stddev: 10.0
 * /s_graphs/g2o_solver_num_iterations: 512
 * /s_graphs/g2o_solver_type: lm_var_cholmod
 * /s_graphs/gps_edge_robust_kernel: NONE
 * /s_graphs/gps_edge_robust_kernel_size: 1.0
 * /s_graphs/gps_edge_stddev_xy: 20.0
 * /s_graphs/gps_edge_stddev_z: 5.0
 * /s_graphs/graph_update_interval: 3.0
 * /s_graphs/imu_acceleration_edge_robust_kernel: NONE
 * /s_graphs/imu_acceleration_edge_stddev: 1.0
 * /s_graphs/imu_orientation_edge_robust_kernel: NONE
 * /s_graphs/imu_orientation_edge_stddev: 1.0
 * /s_graphs/infinite_room_information: 1.0
 * /s_graphs/keyframe_delta_angle: 2.0
 * /s_graphs/keyframe_delta_trans: 2.0
 * /s_graphs/keyframe_window_size: 1
 * /s_graphs/loop_closure_edge_robust_kernel: Huber
 * /s_graphs/loop_closure_edge_robust_kernel_size: 1.0
 * /s_graphs/map_cloud_resolution: 0.05
 * /s_graphs/map_cloud_update_interval: 3.0
 * /s_graphs/map_frame_id: map
 * /s_graphs/max_keyframes_per_update: 10
 * /s_graphs/max_stddev_q: 0.4
 * /s_graphs/max_stddev_x: 5.0
 * /s_graphs/min_edge_interval: 5.0
 * /s_graphs/min_horizontal_inliers: 800
 * /s_graphs/min_plane_points: 100
 * /s_graphs/min_seg_points: 100
 * /s_graphs/min_stddev_q: 0.1
 * /s_graphs/min_stddev_x: 0.1
 * /s_graphs/min_vertical_inliers: 100
 * /s_graphs/odom_frame_id: odom
 * /s_graphs/odometry_edge_robust_kernel: Huber
 * /s_graphs/odometry_edge_robust_kernel_size: 1.0
 * /s_graphs/plane_dist_threshold: 0.35
 * /s_graphs/plane_extraction_frame_id: body
 * /s_graphs/plane_information: 1.0
 * /s_graphs/plane_points_dist: 0.5
 * /s_graphs/plane_visualization_frame_id: body_elevated
 * /s_graphs/reg_correspondence_randomness: 20
 * /s_graphs/reg_max_correspondence_distance: 2.5
 * /s_graphs/reg_max_optimizer_iterations: 20
 * /s_graphs/reg_maximum_iterations: 64
 * /s_graphs/reg_nn_search_method: DIRECT1
 * /s_graphs/reg_num_threads: 8
 * /s_graphs/reg_resolution: 1.0
 * /s_graphs/reg_transformation_epsilon: 0.01
 * /s_graphs/reg_use_reciprocal_correspondences: False
 * /s_graphs/registration_method: FAST_GICP
 * /s_graphs/room_information: 1.0
 * /s_graphs/use_const_inf_matrix: False
 * /s_graphs/use_euclidean_filter: True
 * /s_graphs/use_parallel_plane_constraint: False
 * /s_graphs/use_perpendicular_plane_constraint: False
 * /s_graphs/use_shadow_filter: False
 * /s_graphs/var_gain_a: 20.0
 * /s_graphs/wait_trans_odom2map: False
 * /s_graphs_floor_planner/vertex_neigh_thres: 2
 * /s_graphs_room_segmentor/vertex_neigh_thres: 2
 * /use_sim_time: True
 * /voxblox_skeletonizer/color_mode: lambert
 * /voxblox_skeletonizer/enable_icp: False
 * /voxblox_skeletonizer/esdf_add_occupied_crust: True
 * /voxblox_skeletonizer/esdf_max_distance_m: 5.0
 * /voxblox_skeletonizer/esdf_min_diff_m: 0.0
 * /voxblox_skeletonizer/frame_id: map
 * /voxblox_skeletonizer/generate_by_layer_neighbors: False
 * /voxblox_skeletonizer/icp_refine_roll_pitch: False
 * /voxblox_skeletonizer/input_filepath: /home/hriday/rs_e...
 * /voxblox_skeletonizer/min_gvd_distance: 0.5
 * /voxblox_skeletonizer/min_separation_angle: 0.78
 * /voxblox_skeletonizer/output_filepath: /home/hriday/rs_s...
 * /voxblox_skeletonizer/publish_pointclouds: True
 * /voxblox_skeletonizer/sparse_graph_filepath: /home/hriday/rs_s...
 * /voxblox_skeletonizer/tsdf_voxel_size: 0.2
 * /voxblox_skeletonizer/update_esdf: True
 * /voxblox_skeletonizer/verbose: False
 * /voxblox_skeletonizer/world_frame: map

NODES
  /
    body_to_body_elevated (tf/static_transform_publisher)
    hdl_prefilter (nodelet/nodelet)
    hdl_prefilter_nodelet_manager (nodelet/nodelet)
    hdl_scan_matching_nodelet_manager (nodelet/nodelet)
    map2odom_publisher (s_graphs/map2odom_publisher.py)
    map_to_map_elevated (tf/static_transform_publisher)
    s_graphs (nodelet/nodelet)
    s_graphs_floor_plan_nodelet_manager (nodelet/nodelet)
    s_graphs_floor_planner (nodelet/nodelet)
    s_graphs_nodelet_manager (nodelet/nodelet)
    s_graphs_room_seg_nodelet_manager (nodelet/nodelet)
    s_graphs_room_segmentor (nodelet/nodelet)
    voxblox_skeletonizer (voxblox_skeleton/skeletonizer_realtime)

auto-starting new master
process[master]: started with pid [62300]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 9cb40064-f0a6-11ed-a108-50ebf6bde39a
process[rosout-1]: started with pid [62310]
started core service [/rosout]
process[hdl_prefilter_nodelet_manager-2]: started with pid [62318]
process[hdl_prefilter-3]: started with pid [62319]
process[hdl_scan_matching_nodelet_manager-4]: started with pid [62320]
process[s_graphs_room_seg_nodelet_manager-5]: started with pid [62321]
process[s_graphs_room_segmentor-6]: started with pid [62326]
[ INFO] [1683883390.707672406]: Initializing nodelet with 32 worker threads.
process[s_graphs_floor_plan_nodelet_manager-7]: started with pid [62358]
process[s_graphs_floor_planner-8]: started with pid [62368]
process[s_graphs_nodelet_manager-9]: started with pid [62369]
process[s_graphs-10]: started with pid [62376]
[ INFO] [1683883390.721567054]: Initializing nodelet with 32 worker threads.
process[map2odom_publisher-11]: started with pid [62382]
process[body_to_body_elevated-12]: started with pid [62423]
[ INFO] [1683883390.725722860]: Initializing nodelet with 32 worker threads.
process[voxblox_skeletonizer-13]: started with pid [62468]
process[map_to_map_elevated-14]: started with pid [62470]
[ INFO] [1683883390.732647030]: Loading nodelet /s_graphs of type s_graphs/SGraphsNodelet to manager s_graphs_nodelet_manager with the following remappings:
[ INFO] [1683883390.732849741]: Initializing nodelet with 32 worker threads.
[ INFO] [1683883390.733584898]: /filtered_points -> /hdl_prefilter/filtered_point_cloud
[ INFO] [1683883390.733593648]: /odom -> /platform/odometry
[ INFO] [1683883390.733601758]: /odom2map/initial_pose -> /odom_to_map/initial_pose
[ INFO] [1683883390.733606258]: /s_graphs/map_points -> /s_graphs/map_points
[ INFO] [1683883390.733612898]: /s_graphs/markers -> /s_graphs/markers
[ INFO] [1683883390.733621128]: /s_graphs/odom2map -> /s_graphs/odom_to_map
[ INFO] [1683883390.733626138]: /s_graphs/read_until -> /s_graphs/read_until
[ INFO] [1683883390.736149350]: waitForService: Service [/s_graphs_nodelet_manager/load_nodelet] has not been advertised, waiting...
[ INFO] [1683883390.738944684]: Initializing nodelet with 32 worker threads.
downsample: VOXELGRID 0.1
outlier_removal: RADIUS 2 - 1
[ INFO] [1683883390.757024719]: waitForService: Service [/s_graphs_nodelet_manager/load_nodelet] is now available.
construct solver: lm_var_cholmod
done
registration: FAST_GICP

The process is running in foreground, and the bash prompt does not show up, and then I run rosbag play <bagfile name> --clock in the directory where the bagfile exists.

root@orion:~/s_graphs_ws/src/s_graphs/dataset# rosbag play virtual_dataset --clock
[ INFO] [1683883216.654811818]: Opening virtual_dataset

Waiting 0.2 seconds after advertising topics... done.

Hit space to toggle paused, or 's' to step.
 [RUNNING]  Bag Time:     50.407067   Duration: 8.118067 / 587.473000  

And the bagfile does play. When I open rviz in according to the given commands, and when i try to subscribe to topics like /floor_plan/floor_data or /room_segmentation/room_data or /s_graphs/map_points, there is no visualization. Although /platform/camera/rgb/image_raw does show an output.

Also, when I try to echo these rostopics, there is no chatter on them. For example:

root@orion:~/s_graphs_ws# rostopic echo /floor_plan/floor_data
WARNING: no messages received and simulated time is active.
Is /clock being published?
^C

root@orion:~/s_graphs_ws# rostopic echo /room_segmentation/room_data
WARNING: no messages received and simulated time is active.
Is /clock being published?
^C

root@orion:~/s_graphs_ws# rostopic echo /s_graphs/all_map_planes
WARNING: no messages received and simulated time is active.
Is /clock being published?
^C

root@orion:~/s_graphs_ws# rostopic echo /filtered_points
WARNING: topic [/filtered_points] does not appear to be published yet
^C

root@orion:~/s_graphs_ws# rostopic echo /s_graphs/map_points
WARNING: no messages received and simulated time is active.
Is /clock being published?
^C

root@orion:~/s_graphs_ws# rostopic echo /s_graphs/map_points
WARNING: no messages received and simulated time is active.
Is /clock being published?
^C

The is clock is really being published:

root@orion:~/s_graphs_ws# rostopic echo /clock
clock: 
  secs: 49
  nsecs: 950553614
---
clock: 
  secs: 49
  nsecs: 960608080
---
clock: 
  secs: 49
  nsecs: 970662025

And I think that is why rviz is not able to show the visualisations because there is no output on such topics. Please help.

PS. The command to run the image in a container throws errors, I had to figure out a different way to run it in a container,. I ran it like the following (This shouldn't be the reason of no output, but still):

yash.mehan@orion:~$ xhost +local:root
yash.mehan@orion:~$docker run -it --env="DISPLAY=$DISPLAY" --env="QT_X11_NO_MITSHM=1" --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" --net host --name s_graphs_container sntarg/s_graphs

[PedroS235]

Hello @yash4242, and thank you for your feedback. I have tried to replicate what you have done, I have come to the same problem. I believe that I know where the problem lies, and it's due to the rosbag. From what I have seen, you are using the virtual_dataset, and I believe there are some parameters on the launch file that are not set correctly for the virtual dataset. So one workaround would be to try the real_dataset, which should hopefully work.

One thing that also might save you time is that you don't really need to copy the rosbag inside the docker container. You can run it immediately from your local pc.
Here's the step you can take:

1. Run the docker container as described in the readme.
2. run a roscore on a terminal window in your local pc
3. run the rviz with the config that is found in the repo (on a different terminal)
4. run the s_graphs inside the docker container as described in the readme (and try this time for the real environment)
5. run the real dataset rosbag

Hope it helps! If the problem persists let me know.

[yash4242]

Thank you for your repy! real_dataset works. Also, thanks for sharing the method to avoid the docker cp !
Although the rviz terminal window throws this warning repeatedly:

[ WARN] [1684044376.643416255, 1638975837.524400774]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame body at time 1638975837.300002 according to authority unknown_publisher

• Please suggest how I can get the virtual_dataset to run.
• how can I get the velodyne dataset?
• Also, please suggest how I can get my own data to run.
  • Do I necessarily need a rosbag? how Can i record a rosbag which is compatible with sgraphs+? It would be great if you could provide some launch files and world files so that i can create my own data in Gazebo for example and run sgraphs over it, for experimentation.

Thank you again! :)

[PedroS235]

Hey @yash4242. I think I have found the problem why the virtual dataset is not working. The reason is that the odometry is missing from some reason. Thus, a virtual dataset cannot be used, at least the one we are currently providing. (thanks for noticing that).

Regarding the warning you are getting in rviz, that should not cause any problem.

Since the rosbag is broken, there is no way for it to work, at least that I know of. The workaround would either be to just test it with the real_dataset, or if we find some virtual dataset that is not broken, then we would update the current available to one that works (But no promises). Nevertheless, you can also test with your own rosbags, or even with your real robot if you have one. The only requirements are that you have a pointcloud which is being published to /platform/velodyne_points and also odometry which it expects it to be from odom to body. In case you have a different structure, for example your velodyne points are being published to /velody_points then you can go to the launch file, which you can find inside the docker container, by going to cd /src/s_graphs/launch/ and open the s_graphs.launch file. You will then need to change where we have /platform/velodyne_points to your desired topic name. The same will then be applied to where we have body, you will need to change to your desired frame_id.

Hope it helps!

[hridaybavle]

Hello @yash4242

I have managed to fix the issue with the virtual dataset. There was just a param of compute_odom:=true missing in the README. I have updated it and you can check the new launch command for the virtual dataset here.

If you want to use S-Graphs on your custom dataset it should be quite straightforward. You can create any indoor environment in gazebo as you wish and can use robot simulators like TurtleBot mounted with 3D LiDAR. You just need to make sure you have a base_frame for your robot called base_footprint as well as a frame for the LiDAR (e.g velodyne). So you just need a TF from base_footprint -> velodyne. And you need to make sure that the LiDAR points are published with the topic name /platform/velodyne_points

With the above setup if you just run the launch file with the same params for the virtual dataset S-graphs should work.

Also, note in this current docker version we have not provided the cpp files for some of our code, we plan to make it open-source soon

[yash4242]

Hello @hridaybavle, Thank you for addressing the virtual_dataset issue!
I went through the tutorial you shared, and I believe that's a 2D LiDAR on a turtlebot. SGraphs+ needs a velodyne (vpl 16 or an hdl 32 as i understand) mounted on a turtlebot. I have been looking around quite a lot, and trying to make the robot from turtlebot_description's urdf.xacro file and velodyne's urdf.xacro files. But still I am not able to get the robot to be the state you told.

Will it be possible for the authors to share the robot's files used for creating the virtual dataset please?

We are looking to run SGraphs+ on matterport3D dataset, which is made for Habitat Sim (which has .glb files for each indoor scene), and does not use ROS at all. Can you suggest some ways how we can bridge the Habitat-ROS porting issue?

Thanks a lot!

[hridaybavle]

Hello @yash4242 sorry for the delayed response, did you manage to it get it to work or else I can share with you the xacro we were using? In our simulation, we used the champ and mounted it with the velodyne lidar. Ideally, you should be able to use the Velodyne description in the case of Turtlebot as well.