search

ANYbotics / elevation_mapping

Robot-centric elevation mapping for rough terrain navigation
BSD 3-Clause "New" or "Revised" License
1.31k stars 445 forks source link

D435i issue #155

Closed kiwi-sherbet closed 3 years ago

kiwi-sherbet commented 3 years ago

Hi, I would like to use elevation _mapping package with Realsense d435i. When I ran it, I got the following error messages.

Parameter 'point_cloud_topic' is deprecated, please use 'input_sources' instead

It seems that the configuration for the parameter input_sources is not set properly, and I also found the related terms in the readme file. But, I do not know how I can change input_sources parameter. Where can I set up the configuration for it?

Thanks!

maximilianwulf commented 3 years ago

Hey @kiwi-sherbet,

you can configure input_source with : 

input_sources:
  front: # A name to identify the input source
    type: pointcloud # Supported types: pointcloud
kiwi-sherbet commented 3 years ago

Thank you for answer!

Where (in what file) can I change the lines for configuration?

maximilianwulf commented 3 years ago

Hi @kiwi-sherbet, you can add these changes to your configuration yaml file.

kiwi-sherbet commented 3 years ago

Thank you for reply! Still, I get an error that "Could not configure input source front because no sensor_processor was given." It seems that I am missing some configuration setups. If there is any demo or example for D435i, can I ask for it? It would be great help!

maximilianwulf commented 3 years ago

Sorry, forgot that. You also have to add the sensor processor, like: 

input_sources:
 camera: # A name to identify the input source
    type: pointcloud # Supported types: pointcloud
    topic: /your_topic # your topic
    queue_size: 1
    publish_on_update: true
    sensor_processor: 
      ignore_points_above: .inf
      ignore_points_below: -.inf
      cutoff_min_depth: 0.1
      cutoff_max_depth: 1.5
      type: structured_light # https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=8768489
      normal_factor_a: 0.001063
      normal_factor_b:  0.003949
      normal_factor_c: 0.0
      normal_factor_d: 0.0008278
      normal_factor_e: 1
      lateral_factor: 0.01576 # approx 1 deg
kiwi-sherbet commented 3 years ago

Thank you for information! Can I ask one more question about how to configure gyro data (from IMU)? I still have the following error. So, I would like to know what parameter I can use for pose data setup. 

[ERROR] [-]: Could not get pose information from robot for time 1612910006.554518. Buffer empty?

Thank you!

maximilianwulf commented 3 years ago

Hey, you need to have working state estimation that outputs a pose with a covariance. Do you have such a system running? Elevation mapping does not accept raw IMU measurements.

usamex commented 3 years ago

You have to remap your Odometry type data to PoseWithCovarianceStamped type. You can do it with a simple script like this:

#!/usr/bin/env python
import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import PoseWithCovarianceStamped

def odom_callback(msg, arg):
    publisher = arg
    new_message = PoseWithCovarianceStamped()
    new_message.header = msg.header
    new_message.pose = msg.pose
    publisher.publish(new_message)
if __name__ == '__main__':
    rospy.init_node('odom_to_pose')

    topic_name = rospy.get_param('~odom_topic', '')
    publisher_name = rospy.get_param('~pub_topic_name', '')
    if topic_name == '' or publisher_name == '':
        rospy.logerr("Couldn't get subscriber's topic name or publisher's topic name . Exiting...")
    else:
        publisher = rospy.Publisher(publisher_name, PoseWithCovarianceStamped, queue_size=10)
        rospy.Subscriber(topic_name, Odometry, odom_callback, publisher)
        while not rospy.is_shutdown():
            pass
kiwi-sherbet commented 3 years ago

Thank you for your replies! I am trying to use odometry data from realsense2_camera: https://github.com/IntelRealSense/realsense-ros/wiki/SLAM-with-D435i But, I cannot get any output from elevation_mapping yet, and Rviz visualization shows nothing, as the attached picture. Since the outputs, pose and poinsts from realsense2_camera have the same message formats, I just remapped them.

   ...
    <remap from="/pose" to="/rtabmap/odom/pose"/>
    <remap from="/points" to="/camera/depth/color/points"/>
  </node>

The below are the logs I got.

[ INFO] [1613481860.571951507]: Grid map visualization node started.
[ INFO] [1613481860.572472748]: Grid map visualization node started.
[ INFO] [1613481860.576627216]: grid_map_visualizations: Configured visualization of type 'point_cloud' with name 'elevation_cloud'.
[ INFO] [1613481860.576648146]: grid_map_visualizations: Configured visualization of type 'point_cloud' with name 'sigma_cloud'.
[ INFO] [1613481860.576661531]: grid_map_visualizations: Configured visualization of type 'map_region' with name 'map_region'.
[ INFO] [1613481860.576888493]: grid_map_visualizations: Configured visualization of type 'point_cloud' with name 'elevation_cloud'.
[ INFO] [1613481860.576914362]: grid_map_visualizations: Configured visualization of type 'point_cloud' with name 'upper_bound_cloud'.
[ INFO] [1613481860.576925984]: grid_map_visualizations: Configured visualization of type 'point_cloud' with name 'lower_bound_cloud'.
[ INFO] [1613481860.576938799]: grid_map_visualizations: Configured visualization of type 'map_region' with name 'map_region'.
[ INFO] [1613481860.580047308]: Grid map visualization initialized.
[ INFO] [1613481860.580486813]: Grid map visualization initialized.
[ WARN] [1613481860.595599494]: Could not find the parameter: `algorithm`. Setting to default value: 'area'.
[ WARN] [1613481860.596135413]: Could not find the parameter: `parallelization_enabled`. Setting to default value: 'false'.
[ WARN] [1613481860.596149931]: Could not find the parameter: `thread_number`. Setting to default value: 'automatic'.
[ INFO] [1613481860.596743539]: Elevation mapping node started.
[ INFO] [1613481860.609194190]: Elevation map grid resized to 600 rows and 600 columns.
[ WARN] [1613481860.624354020]: Parameter 'point_cloud_topic' is deprecated, please use 'input_sources' instead.
[ INFO] [1613481860.625212953]: Subscribing to pointcloud: /camera/depth/color/points, queue_size: 1.
[ INFO] [1613481860.627567790]: Elevation mapping node initializing ... 
[ INFO] [1613481861.627861819]: Done initializing.
Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame camera_link at time 1613481862.862974 according to authority unknown_publisher
         at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.5/src/buffer_core.cpp
[ WARN] [1613481862.909370784]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame camera_link at time 1613481862.862974 according to authority unknown_publisher
Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame camera_link at time 1613481862.862974 according to authority unknown_publisher
         at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.5/src/buffer_core.cpp
Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame camera_link at time 1613481862.862974 according to authority unknown_publisher
         at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.5/src/buffer_core.cpp
Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame camera_link at time 1613481875.037906 according to authority unknown_publisher
         at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.5/src/buffer_core.cpp
Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame camera_link at time 1613481875.037906 according to authority unknown_publisher
         at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.5/src/buffer_core.cpp[ WARN] [1613481875.085984857]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame camera_link at time 1613481875.037906 according to authority unknown_publisher

Can I ask for advice for this?

Thank you!

Screenshot 2021-02-16 07:27:13

maximilianwulf commented 3 years ago

Hey @kiwi-sherbet, it seems elevation mapping does not get the input pose. You would get a message saying Received first pose. Can you double-check that you actually get the pose out of your slam module?

kiwi-sherbet commented 3 years ago

Hi @maximilianwulf , I double-checked the input pose in the PoseWithCovarianceStamped type, as @usamex mentioned. I am currently checking /tf topic and configuration at simple_demo_robot.yaml but still got the same results.

askokostic commented 3 years ago

I believe I'm having a similar issue, although my setup is slightly different (T265 for odometry + D455 for depth).

maximilianwulf commented 3 years ago

Hi @kiwi-sherbet, did you manage to make it work?

kiwi-sherbet commented 3 years ago

@maximilianwulf Oh, yes, it additionally requires to setup time stamps. Thank you!

maximilianwulf commented 3 years ago

Great to hear.

wxk514 commented 2 years ago

Have you solved this problem? I met the same problem and followed the above steps. Could you tell me how to solve this problem. [ERROR] [-]: Could not get pose information from robot for time 1612910006.554518. Buffer empty?

wxk514 commented 2 years ago

PoseWithCovarianceStamped

How to set time stamps?thanks!

manavthakkar commented 4 months ago

PoseWithCovarianceStamped

How to set time stamps?thanks!

did you figure out how to set time stamps?