0
$\begingroup$

Rosanswers logo

Hello everyone,

first of all I would like to thank you for such a great forum, it has helped me solve many issues in the past!

I have questions that could be caused by my luck of understanding how the mapping is performed. What i am trying to do is map a wall as it was the ground.

I have a wall crawler and I would like to add the ability to perform autonomous navigation - on the wall - with it.

Crawler setup: chassis with 4 wheels, depth camera and tracking module mounted in front ( Intel realsense d435i, and realsense t265 ) ROS Version: Noetic running on ubuntu 20.04 Packages: depthimage_to_laserscan, gmapping and move_base - basic stuff.

When testing the crawler on the floor mapping and navigation work fine, but when trying to "map the wall" ( effectively rotate the robot by 90 degrees along x axis ), the occupancy map is filled with black cells, as i am guessing it "sees" the wall but identifies it as obstacle.

Other things I tried:

  • using rtabmap_ros as well but I get pretty similar results
  • adding a world frame in between the map and base of the robot and rotating it by 90 degusing a static tf from map to world ( map -> 90 world -> base )

Can this be achieved using gmapping / rtab or any other package? If so, could you please provide any pointers on what changes I have to make in order to achieve this? If not, can you suggest an alternative approach to tackle that?

EDIT1:

Default setup ( map -> t265 odom -> base ), cameras facing the ceiling and fixed frame in RVIZ set to map

image description

Here is the full tf tree of that setup

Here is the 90 map rotation setup (world -> 90 map -> t265 odom -> base ), cameras facing the ceiling and fixed frame in RVIZ set to world.

image description

Here is the full tf tree of that setup

Thank you in advance for your time and effort you might put in trying to assist me.

Best,

Mike


Originally posted by ekptwtos on ROS Answers with karma: 35 on 2021-10-20

Post score: 0


Original comments

Comment by TomSon on 2021-10-20:
Seems that you got a TF initialisation that cause this effect. Your frame "map" shouldn't be rotated. On rtabmap, watch out the parameters that define the height of ground and obstacle, it directly impact the map generation.

Comment by ekptwtos on 2021-10-21:
Hello and thanks for the reply, I have checked the values, but these - as i understand - are good if you have an uneven ground. The only reason i tried to rotate the world frame IRT the map by 90, is because i am trying to "map" the wall, not the floor ( or ground ).

Comment by matlabbe on 2021-10-21:
I think you should do 90 world -> map -> odom -> base, so that all mapping stuff will work in standard z->up. rosrun tf static_transform_publisher 0 0 0 0 -1.57079632679 0 world map 100

Comment by ekptwtos on 2021-10-26:
oh for some reason i didnt get a notification for your comment! will try this next time i get the chance and post results!

Comment by ekptwtos on 2021-10-29:
@matlabbe I did try rotating the map WRT the world, however didn't get any different results :-/ Please see updated post!

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

Hi,

You have to make T265 odometry to work in z->up, y->left, x->forward when the camera is looking up. Add the following just here to add 90 degrees pitch in odometry output:

tf::Transform p;
tf::poseMsgToTF(pose_msg.pose, p);
tf::Quaternion q;
q.setRPY(0, 1.57079632679, 0);
p = tf::Transform(q) * p;
tf::poseTFToMsg(p, pose_msg.pose);

When the robot will be moving up on the wall, the mapping part will think the robot is moving forward, then the occupancy grid should match the wall. You can add world->map 90deg transform if you want the resulting map put back in correct orientation. (rosrun tf static_transform_publisher 0 0 0 0 -1.57079632679 0 world map 100)


Originally posted by matlabbe with karma: 6409 on 2021-11-03

This answer was ACCEPTED on the original site

Post score: 2


Original comments

Comment by ekptwtos on 2021-11-05:
Thank you very much! That is exactly what I had been looking for. So if I am getting this right, the orientation of the odometry frame is the catalyst here. With the properly rotated odom frame I could use any of the mapping / navigation packages? Could this be replicated without the t265 by just using the d435i without any odom frame?

Comment by matlabbe on 2021-11-08:
Yes, this should work with any other slam packages. The odometry rotation may not be needed if your odometry starts already with zero rotation. The problem here is that T265's VIO aligns always odometry with gravity on start because it uses the IMU. For general visual odometry approaches (without IMU), no rotation would be required, just make sure to initialize the odometry when the robot is already on the wall.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.