Open nbertschmann opened 2 years ago
maximilianwulf
commented
2 years ago Hey, setup should look okay,
I guess there might be problem with the tf/pose information, i.e. the following part from the console:
[ERROR] [1648445958.063837999]: Could not get pose information from robot for time 1648445956.360284. Buffer empty?
[ INFO] [1648445958.736752215]: Waiting for tf transformation to be available. (Message is throttled, 10s.)
nbertschmann
commented
2 years ago @maximilianwulf Is there any way to subscribe to a "fake pose" to see if my pose input is the issue?
maximilianwulf
commented
2 years ago Hm, yes, but you have to create the fake pose.
I would actually try the waffle demo first and then just rewire the point clouds, and see if it reacts.
nbertschmann
commented
2 years ago @maximilianwulf Thank you for the advice - I just tried this out.
I'm still getting the same issue where the elevation_mapping/elevation_map topic is being published, but elevation_mapping/elevation_map_raw is not.
Here is what I modified in the turtlesim3_wafffle_demo.launch file. The input to the down-sample filter is now /zed/zed_node/point_cloud/cloud_registered
<!-- Run a passthrough filter to down-sample the sensor point cloud.-->
<node pkg="nodelet" type="nodelet" name="pcl_manager" args="manager" output="screen"/>
<node pkg="nodelet" type="nodelet" name="voxel_grid" args="load pcl/VoxelGrid pcl_manager" output="screen">
<remap from="~input" to="/zed/zed_node/point_cloud/cloud_registered"/>
<remap from="~output" to="/camera/depth/points_downsampled"/>
<rosparam>
filter_field_name: z
filter_limit_min: 0.01
filter_limit_max: 6.0 <!-- Limit the range of points to forward to 6 meters in viewing direction-->
filter_limit_negative: False
leaf_size: 0.05 <!-- Limit the density to 5 cm-->
</rosparam>
</node>rqt_graph:
RViz Window:
Terminal Output:
nbertsch@nbertsch-desktop:~/catkin_ws$ roslaunch elevation_mapping_demos turtlesim3_waffle_demo2.launch
... logging to /home/nbertsch/.ros/log/0ea855d8-b054-11ec-a556-1c1bb5e39bda/roslaunch-nbertsch-desktop-10065.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.
xacro: in-order processing became default in ROS Melodic. You can drop the option.
started roslaunch server http://nbertsch-desktop:46879/
SUMMARY
========
PARAMETERS
* /elevation_mapping/enable_visibility_cleanup: False
* /elevation_mapping/fused_map_publishing_rate: 0.5
* /elevation_mapping/init_submap_height_offset: 0.01
* /elevation_mapping/initialization_method: 0
* /elevation_mapping/initialize_elevation_map: False
* /elevation_mapping/input_sources/zed/publish_on_update: True
* /elevation_mapping/input_sources/zed/queue_size: 1
* /elevation_mapping/input_sources/zed/sensor_processor/apply_voxelgrid_filter: True
* /elevation_mapping/input_sources/zed/sensor_processor/cutoff_max_depth: 20
* /elevation_mapping/input_sources/zed/sensor_processor/cutoff_min_depth: 0.3
* /elevation_mapping/input_sources/zed/sensor_processor/depth_to_disparity_factor: 47.3
* /elevation_mapping/input_sources/zed/sensor_processor/ignore_points_above: 1.4
* /elevation_mapping/input_sources/zed/sensor_processor/lateral_factor: 0.001376915
* /elevation_mapping/input_sources/zed/sensor_processor/p_1: 0.03287
* /elevation_mapping/input_sources/zed/sensor_processor/p_2: -0.0001276
* /elevation_mapping/input_sources/zed/sensor_processor/p_3: 0.485
* /elevation_mapping/input_sources/zed/sensor_processor/p_4: 399.1046
* /elevation_mapping/input_sources/zed/sensor_processor/p_5: 6.735e-06
* /elevation_mapping/input_sources/zed/sensor_processor/type: stereo
* /elevation_mapping/input_sources/zed/sensor_processor/voxelgrid_filter_size: 0.1
* /elevation_mapping/input_sources/zed/topic: /camera/depth/poi...
* /elevation_mapping/input_sources/zed/type: pointcloud
* /elevation_mapping/length_in_x: 6.0
* /elevation_mapping/length_in_x_init_submap: 1.0
* /elevation_mapping/length_in_y: 6.0
* /elevation_mapping/length_in_y_init_submap: 1.0
* /elevation_mapping/mahalanobis_distance_threshold: 2.5
* /elevation_mapping/map_frame_id: odom
* /elevation_mapping/margin_init_submap: 0.3
* /elevation_mapping/max_variance: 0.05
* /elevation_mapping/min_update_rate: 2.0
* /elevation_mapping/min_variance: 0.0001
* /elevation_mapping/multi_height_noise: 0.001
* /elevation_mapping/position_x: 0.0
* /elevation_mapping/position_y: 0.0
* /elevation_mapping/postprocessor_pipeline: [{'type': 'gridMa...
* /elevation_mapping/resolution: 0.1
* /elevation_mapping/robot_base_frame_id: base_footprint
* /elevation_mapping/robot_motion_map_update/covariance_scale: 0.01
* /elevation_mapping/robot_pose_cache_size: 200
* /elevation_mapping/robot_pose_with_covariance_topic: /base_footprint_pose
* /elevation_mapping/scanning_duration: 1.0
* /elevation_mapping/sensor_processor/ignore_points_above: 0.4
* /elevation_mapping/surface_normal_positive_axis: z
* /elevation_mapping/target_frame_init_submap: base_footprint
* /elevation_mapping/time_offset_for_point_cloud: 0.0
* /elevation_mapping/time_tolerance: 1.0
* /elevation_mapping/track_point_frame_id: base_footprint
* /elevation_mapping/track_point_x: 0.0
* /elevation_mapping/track_point_y: 0.0
* /elevation_mapping/track_point_z: 0.0
* /elevation_mapping/visibility_cleanup_rate: 1.0
* /gazebo/enable_ros_network: True
* /robot_description: <?xml version="1....
* /rosdistro: melodic
* /rosversion: 1.14.13
* /use_sim_time: True
* /voxel_grid/filter_field_name: z
* /voxel_grid/filter_limit_max: 6.0
* /voxel_grid/filter_limit_min: 0.01
* /voxel_grid/filter_limit_negative: False
* /voxel_grid/leaf_size: 0.05
* /waffle_pose_publisher/from_frame: odom
* /waffle_pose_publisher/to_frame: base_footprint
NODES
/
elevation_mapping (elevation_mapping/elevation_mapping)
gazebo (gazebo_ros/gzserver)
pcl_manager (nodelet/nodelet)
rviz (rviz/rviz)
spawn_urdf (gazebo_ros/spawn_model)
voxel_grid (nodelet/nodelet)
waffle_pose_publisher (elevation_mapping_demos/tf_to_pose_publisher.py)
waffle_state_publisher (robot_state_publisher/robot_state_publisher)
auto-starting new master
process[master]: started with pid [10087]
ROS_MASTER_URI=http://localhost:11311
setting /run_id to 0ea855d8-b054-11ec-a556-1c1bb5e39bda
process[rosout-1]: started with pid [10103]
started core service [/rosout]
process[gazebo-2]: started with pid [10112]
process[spawn_urdf-3]: started with pid [10128]
process[waffle_state_publisher-4]: started with pid [10133]
process[waffle_pose_publisher-5]: started with pid [10134]
process[pcl_manager-6]: started with pid [10136]
process[voxel_grid-7]: started with pid [10148]
process[elevation_mapping-8]: started with pid [10158]
process[rviz-9]: started with pid [10177]
[ INFO] [1648663566.765015416]: Loading nodelet /voxel_grid of type pcl/VoxelGrid to manager pcl_manager with the following remappings:
[ INFO] [1648663566.797206592]: /voxel_grid/input -> /zed/zed_node/point_cloud/cloud_registered
[ INFO] [1648663566.797273989]: /voxel_grid/output -> /camera/depth/points_downsampled
[ INFO] [1648663566.866433546]: waitForService: Service [/pcl_manager/load_nodelet] has not been advertised, waiting...
[ INFO] [1648663566.880825940]: Initializing nodelet with 4 worker threads.
[ INFO] [1648663566.946185713]: waitForService: Service [/pcl_manager/load_nodelet] is now available.
[ WARN] [1648663567.827211072]: Could not find the parameter: `algorithm`. Setting to default value: 'area'.
[ WARN] [1648663567.838409218]: Could not find the parameter: `parallelization_enabled`. Setting to default value: 'false'.
[ WARN] [1648663567.838569483]: Could not find the parameter: `thread_number`. Setting to default value: 'automatic'.
[ INFO] [1648663567.851380742]: Elevation mapping node started.
[ INFO] [1648663568.006554899]: Elevation map grid resized to 60 rows and 60 columns.
[ INFO] [1648663568.838019761]: Subscribing to pointcloud: /camera/depth/points_downsampled, queue_size: 1.
[ INFO] [1648663568.983613143]: Elevation mapping node initializing ...
[ INFO] [1648663569.936265999]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1648663569.950289682]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
[ INFO] [1648663573.107441974]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[ INFO] [1648663574.735791469, 0.001000000]: Camera Plugin: Using the 'robotNamespace' param: '/'
[ INFO] [1648663574.747741709, 0.001000000]: Camera Plugin (ns = /) <tf_prefix_>, set to ""
[ INFO] [1648663575.251642035, 0.001000000]: Laser Plugin: Using the 'robotNamespace' param: '/'
[ INFO] [1648663575.279057755, 0.001000000]: Starting Laser Plugin (ns = /)
[ INFO] [1648663575.299410022, 0.001000000]: Laser Plugin (ns = /) <tf_prefix_>, set to ""
[ INFO] [1648663575.337552671, 0.001000000]: Physics dynamic reconfigure ready.
[ INFO] [1648663575.378494502, 0.001000000]: Starting plugin DiffDrive(ns = //)
[ INFO] [1648663575.379390461, 0.001000000]: DiffDrive(ns = //): <rosDebugLevel> = na
[ INFO] [1648663575.382296886, 0.001000000]: DiffDrive(ns = //): <tf_prefix> =
[ INFO] [1648663575.386357558, 0.001000000]: DiffDrive(ns = //): Advertise joint_states
[ INFO] [1648663575.390057284, 0.001000000]: DiffDrive(ns = //): Try to subscribe to cmd_vel
[ INFO] [1648663575.451213670, 0.001000000]: DiffDrive(ns = //): Subscribe to cmd_vel
[ INFO] [1648663575.466507272, 0.001000000]: DiffDrive(ns = //): Advertise odom on odom
[spawn_urdf-3] process has finished cleanly
log file: /home/nbertsch/.ros/log/0ea855d8-b054-11ec-a556-1c1bb5e39bda/spawn_urdf-3*.log
[ INFO] [1648663581.451937249, 1.001000000]: Successfully launched node.
[ INFO] [1648663666.591700287, 22.692000000]: First corresponding point cloud and pose found, elevation mapping started.
[ INFO] [1648663666.920748203, 22.777000000]: Waiting for tf transformation to be available. (Message is throttled, 10s.)
[ INFO] [1648663712.469113885, 32.801000000]: Waiting for tf transformation to be available. (Message is throttled, 10s.)
[ INFO] [1648663761.629760742, 42.864000000]: Waiting for tf transformation to be available. (Message is throttled, 10s.)Looks like it may be a TF issue from the ZED camera? Any idea how this could be fixed?
maximilianwulf
commented
2 years ago Waiting for tf transformation to be available. (Message is throttled, 10s.)
My best guess would be that the ZED driver does not properly publish the static tf tree. Sorry I have no experience with that driver.
Musab-Imadeldin
commented
1 year ago Hi, I'm facing the same issue @nbertschmann. What were the steps you took to resolve this?
I'm modifying the simple demo launch and I'm trying to set the d435i camera I have. A comment would be appreciated.
hsurya08
commented
1 year ago Hi, I am facing the same issue @nbertschmann. Are you able to find the solution?
I have been attempting to use ZED point cloud data to test out the elevation_mapping package.
First, here is the output I get using
rqt_graph. The elevation_mapping node is subscribed to the down-sampled point cloud (type sensor_msgs/PointCloud2), the pose of the ZED camera (type geometry_msgs/PoseWithCovariance), and tf.In RViz, the pointcloud from the ZED camera displays properly, but the elevation mapping data from elevation_mapping_raw is not displaying. Below is a screenshot of my RViz window:
My bagfile info is below. As you can see, the elevation mapping node published 31 /elevation_mapping/elevation_map topic messages. However, it does not seem that any /elevation_mapping/elevation_map_raw topic messages were published.
To add to this - on RViz, I can see that the elevation_mapping/elevation_map topic messages are being published successfully:
However, it appears that the elevation_mapping/elevation_map_raw messages are not being published:
Not sure if this helps, but here is the output from
roslaunch elevation_mapping_demos zed_test2.launch(modified version of simple_demo to test elevation mapping with ZED input):I would be greatly appreciated if you can provide some insight as to why the elevation_mapping/elevation_map_raw is not being published.