#!/usr/bin/env python

    import rospy, math

    from costmap_converter.msg import ObstacleArrayMsg, ObstacleMsg

    from geometry_msgs.msg import PolygonStamped, Point32

    def publish_obstacle_msg():

      rospy.init_node("test_obstacle_msg")

      pub = rospy.Publisher('/test_optim_node/obstacles', ObstacleArrayMsg, queue_size=)

     obstacle_msg = ObstacleArrayMsg()

     obstacle_msg.header.stamp = rospy.Time.now()

     obstacle_msg.header.frame_id = "odom" # CHANGE HERE: odom/map

     # Add point obstacle

     obstacle_msg.obstacles.append(ObstacleMsg())

     obstacle_msg.obstacles[].id =

     obstacle_msg.obstacles[].polygon.points = [Point32()]

     obstacle_msg.obstacles[].polygon.points[].x = 1.5

     obstacle_msg.obstacles[].polygon.points[].y =

     obstacle_msg.obstacles[].polygon.points[].z = 

     # Add line obstacle

     obstacle_msg.obstacles.append(ObstacleMsg())

     obstacle_msg.obstacles[].id =

     line_start = Point32()

     line_start.x = -2.5

     line_start.y = 0.5

     line_end = Point32()

     line_end.x = -2.5

     line_end.y =

     obstacle_msg.obstacles[].polygon.points = [line_start, line_end]

     # Add polygon obstacle

     obstacle_msg.obstacles.append(ObstacleMsg())

     obstacle_msg.obstacles[].id =

     v1 = Point32()

     v1.x = -

     v1.y = -

     v2 = Point32()

     v2.x = -0.5

     v2.y = -1.5

     v3 = Point32()

     v3.x =

     v3.y = -

     obstacle_msg.obstacles[].polygon.points = [v1, v2, v3]

     r = rospy.Rate() # 10hz

     t = 0.0

     while not rospy.is_shutdown():

       # Vary y component of the point obstacle

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

       t = t + 0.1

       pub.publish(obstacle_msg)

       r.sleep()

   if __name__ == '__main__':

     try:

       publish_obstacle_msg()

     except rospy.ROSInterruptException:

       pass

~<name>/min_obstacle_dist: Desired minimal distance from obstacles

~<name>/include_costmap_obstacles: Deactivate costmap obstacles completely

~<name>/costmap_obstacles_behind_robot_dist: Maximum distance behind the robot searched for occupied costmap cells.

~<name>/obstacle_poses_affected: Specify how many trajectory configurations/poses should be taken into account next to the closest one.

~<name>/weight_obstacle: Optimization weight for keeping a distance to obstacles.

~<name>/footprint_model: The robot footprint model