homotopy class can not work when there're many obstacles

[FreeCars]

Firstly,thank you for your great job! I use teb with default params, and enable_homotopy_class_planning is true, there is no problem when I run roslaunch teb_local_planner test_optim_node.launch Then, I run rosrun teb_local_planner publish_test_obstacles.py Everything is still ok. but, you kown, now there are only a few obstacles. I modified publish_test_obstacles.py, and 100 dot obstacles are added to the screen, and delete the line and polygon obstacles. Things go wrong, it seems that the homotopy algorithm could not search different trajectories any more, and all trajectories tend to gather together.(now 4 homotopies are all used.)

by tracing the code, I found that when caculating HSignature, Al is 0 at last,maybe that's because
Al = Al / diff;

Could you please give me some ideas about this?

the code is as follows(just for experiment purpose, the same problem also occurs in real robot situation). `def publish_obstacle_msg(): pub = rospy.Publisher('/test_optim_node/obstacles', ObstacleArrayMsg, queue_size=1)

pub = rospy.Publisher('/p3dx/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1)

rospy.init_node("test_obstacle_msg")

obstacle_msg = ObstacleArrayMsg() obstacle_msg.header.stamp = rospy.Time.now() obstacle_msg.header.frame_id = "odom" # CHANGE HERE: odom/map

len = 100

for i in range(0, len): obstacle_msg.obstacles.append(ObstacleMsg()) obstacle_msg.obstacles[i].id = i obstacle_msg.obstacles[i].polygon.points = [Point32()] obstacle_msg.obstacles[i].polygon.points[0].x = 0.02*i - 2 obstacle_msg.obstacles[i].polygon.points[0].y = 4 obstacle_msg.obstacles[i].polygon.points[0].z = 0

r = rospy.Rate(1) # 10hz t = 0.0 while not rospy.is_shutdown():

# Vary y component of the point obstacle

for i in range(0, len):

obstacle_msg.obstacles[i].polygon.points[0].y = 3*math.sin(t)

t = t + 0.05

pub.publish(obstacle_msg)

r.sleep()

if name == 'main': try: publish_obstacle_msg() except rospy.ROSInterruptException: pass`