Perception Pipeline Tutorial: Octomap not loading

[KarimHP]

Description

Overview of your issue here.

Your environment

• ROS Distro: Noetic
• OS Version: Ubuntu 20.04
• Camera: Realsense Depth Camera D435i

Steps to reproduce

Im trying to integrate an octomap into my moveit/rviz setup. For this im following this tutorial: https://ros-planning.github.io/moveit_tutorials/doc/perception_pipeline/perception_pipeline_tutorial.html

Now the Problem is: If i start the demo from the tutorial with the pregiven rosbag that contains the octomap data:

  <!-- Play the rosbag that contains the pointcloud data -->
  <node pkg="moveit_tutorials" type="bag_publisher_maintain_time" name="point_clouds" />

Everything works fine. Here is a screenshot

Now im using an intel realsense Depth Camera D435i with ROS and want to use the pointcloud topic to create an octomap in moveit. This does not work. It already worked with other methods (open3d etc.) so i doubt its about the cameras pointcloud output. When i echo the pointcloud output it looks correct. The only difference to the pregiven rosbag i noticed was that the "isDense" Attribute was True. So i wrote a little script that sets it to false and republishes it.

#!/usr/bin/env python

import rospy
from sensor_msgs.msg import PointCloud2

def callback(point_cloud):
    # Modify the is_dense attribute
    point_cloud.is_dense = False

    # Publish the modified point cloud
    pub.publish(point_cloud)

def main():
    global pub
    rospy.init_node('change_is_dense_node', anonymous=True)

    # Set the input and output topics
    input_topic = "/camera/depth/color/points" # Replace with your point cloud topic
    output_topic = "/camera/depth_registered/points"

    # Subscribe to the input point cloud topic
    rospy.Subscriber(input_topic, PointCloud2, callback)

    # Create a publisher for the modified point cloud
    pub = rospy.Publisher(output_topic, PointCloud2, queue_size=10)

    rospy.spin()

if __name__ == '__main__':
    main()

Now when i do rostopic echo /camera/depth_registered/points", this is the output (which seems to be correct):

So i wrote it in my sensors_kinect_pointcloud.yaml:

sensors:
  - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
    #point_cloud_topic: /camera/depth/color/points
    point_cloud_topic: /camera/depth_registered/points
    max_range: 5.0
    point_subsample: 1
    padding_offset: 0.1
    padding_scale: 1.0
    max_update_rate: 1.0
    filtered_cloud_topic: filtered_cloud

As i already said this topic works with other methods like open3d. So im thinking im doing something wrong in move it.

I call move it with:

roslaunch ur5_moveit_config moveit_planning_execution.launch sim:=false robot_ip:=192.168.0.100 limited:=true

and then i start rviz with

roslaunch ur5_moveit_config  moveit_rviz.launch config:=true

Now as you can see in the screenshot, even if i choose the corrent topics which is /camera/depth_registered/points, no pointcloud shows up. I also dont understand the error that i get which is:

Transform [sender=unknown_publisher]
For frame [camera_depth_optical_frame]: No transform to fixed frame [base_link]. TF error: [Could not find a connection between 'base_link' and 'camera_depth_optical_frame' because they are not part of the same tree.Tf has two or more unconnected trees.] 

Why is no octomap showing up?

Thanks in advance for every help!

[welcome[bot]]

Thanks for reporting an issue. Because we're a volunteer community, providing a pull request with suggested changes is always welcomed.

[rhaschke]

Changing the is_dense flag of your PC message does't make sense. If your camera sets this flag, it is for a reason. The error you are seeing perfectly explains your problem: TF cannot determine a transform from camera_depth_optical_frame to base_link. Because of that, no ROS component (neither rviz nor MoveIt) can transform the PC into some robot-related frame. You need to publish a (static) transform describing the location of your camera w.r.t. the robot!

[KarimHP]

First of all, thank you for answering. Sadly i dont fully understand. So the mistake is that i use the base_link as a fixed frame? How do i publish a static transform describing the location of my camera? Is this explained somewhere in the tutorials?

[rhaschke]

http://wiki.ros.org/tf2_ros#static_transform_publisher