Robot camera does not detect obstacles (Gazebo)

[lucasbrodo]

Hi @qiayuanl, Thanks for this rich work.

I try adding some obstacles in empty_world but the robot does no see them. Looks like the convex segmented planes are properly displayed (but not the entire grid map). I'm using your legged_perceptive_controllers/config/rviz.rviz file.

I have removed the T265 camera in robot.xacro.

Thank you very much for your help.

[YifuYuan]

Hi @lucasbrodo,

I'm getting an error message with the TF tree when loading the controller and cannot bring up the robot in Rviz properly. Did you encounter the same issue and how did you solve it? Thanks!

[lucasbrodo]

Hi @YifuYuan, Have you also correctly started the perceptive_controller ?

rosservice call /controller_manager/switch_controller "start_controllers: ['controllers/perceptive_controller']                   
stop_controllers: ['']
strictness: 0
start_asap: false
timeout: 0.0" 

[YifuYuan]

@lucasbrodo Thank you so much for your timely reply! That's exactly the problem and now I can properly run the controller.

I also saw you mentioned that uninstalling nodelet can break the elevation_mapping, and that also happened to me. How did you solve that? Thanks!

[lucasbrodo]

Hey @YifuYuan, I'm still figuring out how to solve it properly :(

[lucasbrodo]

@YifuYuan Have you found a way to resolve this problem ?

[YifuYuan]

@lucasbrodo Yes, I found out that some of the packages are messed up after uninstalling the nodelet, including elevation_mapping, realsense SDK, and realsense_gazebo_plugin. So I rebuilt all the required dependencies and now it works fine.

[lucasbrodo]

Hi @YifuYuan, Good to know ! I will try again tomorrow by rebuilding every packages. Do you see the grid map correctly ? Is the robot able to climb obstacles ?

[YifuYuan]

@lucasbrodo Good luck! Yes, the entire perceptive locomotion pipeline works well. The only problem I have right now is that the elevation map update frequency is only around 6-7Hz due to the computational power limit, which is not high enough to ensure a stable mapping during dynamic locomotion. I'm still working on that part by optimizing each stage, like downsampling the raw point cloud, or maybe just switching to a more powerful desktop :). I would like to know how much elevation map update frequency you will get after you get the framework working!

[nanbwrn]

Have any of you encountered this situation, and how should it be resolved? Thank you

Initially, the robot can walk normally, but suddenly, it behaves strangely and falls as if it has reached some kind of limit. The following error appeared in the terminal:[Ocs2 MPC thread] Erro : GridMap::atPosition(...): Position is out of range.

[lucasbrodo]

Hi @YifuYuan, I rebuilt all the packages but still unable to see the grid map. I'm using Ubuntu 20.04 with ROS Noetic. I'm using catkin build for packages installation.

[YifuYuan]

@lucasbrodo My setup is the same as yours. I guess you can check the pipeline stage by stage, making sure the camera publishes the point cloud msg properly and the elevation mapping node subscribes exactly to that point cloud data msg. As from your screenshot, I can only see the foot placement constraint polygon but not the elevation map. Hope this helps.

[nanbwrn]

hi，@YifuYuan ，I found that there is no point cloud information from the camera in the topics I published. I also couldn't locate the camera's point cloud data in RViz. Therefore, I am unable to display the same visuals as yours in RViz.

[YifuYuan]

Hi @nanbwrn, Are you using D435 for the perception? Did you check if the realsense sdk and realsense_gazebo_plugin are been installed and built properly?

[nanbwrn]

hi，@YifuYuan Thank you. I have installed the two files you mentioned, and I found the topic name for the camera in the topic list. However, when I execute the 'rostopic info /front_camera/depth/color/points' command in the terminal, I noticed that the program is not publishing camera information. It indicates there is a subscription but no publication. I have carefully reviewed the code and haven't identified any issues. Could you please advise on how to resolve this?

[YifuYuan]

Hi @nanbwrn, By default, I believe the d435 outputs the point cloud data to the rostopic /camera/depth/color/points. You could check if this is the case for you and if such rostopic publishes valid point cloud data. After this, to get the elevation mapping working, you should also change the topic in the elevation_mapping.yaml file accordingly.

[nanbwrn]

Thank you ,@YifuYuan Thank you for your prompt response. I am continuing to experiment, hoping to achieve a simulation result similar to yours. I am using the D435 camera, and the topic name in the 'elevation_mapping.yaml' file is left as the default in the code, which matches the topics published by the D435 camera. So, I haven't made any changes for now. However, strangely, although I can find a camera topic in the terminal, I cannot receive point cloud data published by the D435 camera in RViz. This issue might lead to the robot being unable to recognize obstacles?

[YifuYuan]

Do you have /camera/depth/color/points under rostopic list? And does it publish data? You can also check this by adding pointcloud2 visualization in Rviz and selecting from the drop-down topic menu.

[nanbwrn]

In the list of topics, it does appear. However, when adding point cloud data in RViz, the display remains blank, and obstacles still cannot be visualized in the RViz simulation.

[YifuYuan]

@nanbwrn Then I think the problem is your realsense camera is not properly set up. You can start by going over realsense-ros official tutorial or demo to bring up the d435 in the gazebo solely, like the following demo:

[nanbwrn]

hi，@YifuYuan Thank you for your response. Following the tutorial on realsense-ros, I have conducted some small simulations, and now I can achieve a simulation effect similar to yours.

[tony-hj]

Hi，@lucasbrodo I saw that you removed T265. Is there any way to make T265 work? I got an error when I added T265. Do you know how to solve this problem, thank you very much. [ERROR] [1709299206.611116230, 18.830000000]: Exception thrown while initializing controller 'controllers/perceptive_controller'. Mesh package://realsense2_description/meshes/t265.stl could not be found. [ERROR] [1709299206.616946789, 18.832000000]: Initializing controller 'controllers/perceptive_controller' failed I have put the t265.stl file into the corresponding folder, but this errror stoll exists.

[liukaishu]

@YifuYuan The only problem I have right now is that the elevation map update frequency is only around 6-7Hz due to the computational power limit, which is not high enough to ensure a stable mapping during dynamic locomotion.

Hello, have you solved this problem? Now I also encounter this problem.

[YifuYuan]

@liukaishu Hi, in Sim, I managed to boost the frequency to 12-14Hz on a i7-11800H by shrinking the size of the elevation mapping to 3*3m and adding pointcloud downsampling filter for the input of the elevation mapping node. Other than that, I didn't find anything else that could optimize the update rate effectively. My CPU usage is running 100% all time, so I assume the computational power limit is the source of the issue.

[Briser-bit]

@lucasbrodo Thank you so much for your timely reply! That's exactly the problem and now I can properly run the controller.

Hi, @YifuYuan I have encountered the same problem as you. How did you resolve it in the end? And there was also another issue that arose, have you ever encountered it before: [ERROR] [1711550169.939330669]: Tried to advertise a service that is already advertised in this node [/point_cloud_manager/reload_model]

[Briser-bit]

Hello, I found that there is no d435 camera in my Gazebo, only t265. Does anyone know what's going on?

[YifuYuan]

Hi @Briser-bit ,

Did you try to follow the launching instructions in the legged_control? Launch the controller first and then the controller manager. For the camera issue, did you install the realsense_gazebo_plugin? You probably also need to modify your launch file to switch from t265 to d435i.

[IanYe0403]

Hello, I found that there is no d435 camera in my Gazebo, only t265. Does anyone know what's going on?

where did you find t265.stl bro, my t265.stl is too large

[Briser-bit]

Hello, I found that there is no d435 camera in my Gazebo, only t265. Does anyone know what's going on?

where did you find t265.stl bro, my t265.stl is too large

The t265.stl I found is also very large. You can add a scaling factor when importing the model in the URDF or XACRO file.