sikang / mpl_ros

A ROS wrapper for trajectory planning based on motion primitives
Apache License 2.0
588 stars 150 forks source link

VoxelMap center to quadrotor's position #5

Closed mktk1117 closed 6 years ago

mktk1117 commented 6 years ago

Hi! Thank you for your library!

I tried to use this planner with rotors simulator with gazebo. Then, I want to set the center of the VoxelMap to a quadrotor's position. When I just set the origin with VoxelMapUtil's setMap function, it just shifted the map. selection_003

How can I fix this?

Thank you!

sikang commented 6 years ago

Hi Takahiro,

The origin in setMap function is the lower left corner of the voxel map. Let center be you current robot position, dim be the dimension of the map (dim in meter instead of integer). Then the origin should be center - 0.5 * dim. When the robot is moving, you have to update origin accordingly.

Sikang

On Fri, Jul 27, 2018 at 11:34 AM, Takahiro Miki notifications@github.com wrote:

Hi! Thank you for your library!

I tried to use this planner with rotors simulator with gazebo. Then, I want to set the center of the VoxelMap to a quadrotor's position. When I just set the origin with VoxelMapUtil's setMap function, it just shifted the map. [image: selection_003] https://user-images.githubusercontent.com/12872984/43330025-fc150dd6-91fc-11e8-868f-5b129d2f5ea9.png

How can I fix this?

Thank you!

— You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub https://github.com/sikang/mpl_ros/issues/5, or mute the thread https://github.com/notifications/unsubscribe-auth/ABccl1K0xvye4FgaqDY-WoQwziZbjsIMks5uKzMUgaJpZM4VjtG_ .

mktk1117 commented 6 years ago

Thank you Sikang!

Yes, I did center - 0.5dim.

But the problem is the voxel map takes the position of the points from the world frame. For example, if an obstacle is at (x=1, y=0) and the center of the voxel map is (x=-1, y=0), the obstacle's position in the voxel map should be (x=2, y=0) (looking from a quadrotor's position) But, it becomes just (x=1, y=0) So, when the robot moves, only the origin of the voxel map changes and the voxels inside do not change.

mktk1117 commented 6 years ago

Hi Sikang!

I found the solution. I didn't call VoxelGrid::allocate before adding the cloud. After I called this fuction to set the origin, the problem fixed.

Thank you!

sikang commented 6 years ago

:+1: