Dynamic Obstacles on MPC Local Planner

[brilianputraa]

Hi we try to implement a dynamic obstacle publisher for the MPC Local planner based on the object detection that we get from Lidar, so we publish a /ObstacleArrayMsg message type to /move_base/MpcLocalPlannerROS/obstacles topic based on this tutorial incorporating dynamic obstacle tutorial from TEB Local Planner. But we only published the position and pose of the obstacles (polygon shape) without the velocity of the obstacles.

We also have set enable_dynamic_obstacles: True

However we have following problems:

• When we tested it in simulation through gazebo, the obstacle appears in the RViz but when we run the MPC local planner to plan until the goal points, the MPC local path go through the obstacle and not doing any replanning
  

• If we do the test_optim_node and publish the dynamic obstacle message, the MPC local planner wants to replan its path
  

Also my question, do we need to add velocity to the dynamic obstacle to make it works? Thank you