## For instruction on writing tutorials
## http://www.ros.org/wiki/WritingTutorials
####################################
##FILL ME IN
####################################
## for a custom note with links:
## note =
## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links
## note.0= [[teb_local_planner/Tutorials/Setup and test Optimization|Set up and test Optimization]]
## descriptive title for the tutorial
## title = Incorporate customized Obstacles
## multi-line description to be displayed in search
## description = In this tutorial you will learn how to take polygon-shaped obstacles published from other nodes into account.
## the next tutorial description (optional)
## next =
## links to next tutorial (optional)
## next.0.link=[[teb_local_planner/Tutorials/Incorporate dynamic obstacles|Incorporate dynamic obstacles]]
## next.1.link=
## what level user is this tutorial for
## level= IntermediateCategory
## keywords = Trajectory Optimization Local Planner Navigation Obstacles Polygon
####################################
<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

<<TableOfContents(4)>>

== Introduction ==

For some applications you might not want to rely on a costmap or you want to add other than point-obstacles.
In that case you can send your own obstacle list to the teb_local_planner package using a specific topic (`~<name>/obstacles`).

The underlying message-type <<MsgLink(costmap_converter/ObstacleArrayMsg)>> is part of the [[costmap_converter]] package.
The message specifies the following obstacle types:
 *Point obstacle: provide a polygon with a single vertex
 *Circular obstacle: provide a polygon with a single vertex and a non-zero radius value.
 *Line obstacle: provide a polygon with two vertices
 *Polygon obstacle: provide a polygon with more than 2 vertices; the polygon will be considered to be closed between the last and the first vertex.


== Write a simple Obstacle Publisher ==

In the following, we will create a small python node that publishes a few obstacles.
For the planning part, we will run the `test_optim_node` described in the tutorial [[teb_local_planner/Tutorials/Setup and test Optimization|Set up and test Optimization]]. 
Alternatively, you could also test your publisher using a properly configured navigation launch file according to [[teb_local_planner/Tutorials/Configure and run Robot Navigation|Configure and run Robot Navigation]].

Create a python node called `publish_obstacles.py`:

{{{
#!python 
#!/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=1)

  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[0].id = 0
  obstacle_msg.obstacles[0].polygon.points = [Point32()]
  obstacle_msg.obstacles[0].polygon.points[0].x = 1.5
  obstacle_msg.obstacles[0].polygon.points[0].y = 0
  obstacle_msg.obstacles[0].polygon.points[0].z = 0

  # Add line obstacle
  obstacle_msg.obstacles.append(ObstacleMsg())
  obstacle_msg.obstacles[1].id = 1
  line_start = Point32()
  line_start.x = -2.5
  line_start.y = 0.5
  line_end = Point32()
  line_end.x = -2.5
  line_end.y = 2
  obstacle_msg.obstacles[1].polygon.points = [line_start, line_end]
  
  # Add polygon obstacle
  obstacle_msg.obstacles.append(ObstacleMsg())
  obstacle_msg.obstacles[1].id = 2
  v1 = Point32()
  v1.x = -1
  v1.y = -1
  v2 = Point32()
  v2.x = -0.5
  v2.y = -1.5
  v3 = Point32()
  v3.x = 0
  v3.y = -1
  obstacle_msg.obstacles[2].polygon.points = [v1, v2, v3]

  r = rospy.Rate(10) # 10hz
  t = 0.0
  while not rospy.is_shutdown():
    
    # Vary y component of the point obstacle
    obstacle_msg.obstacles[0].polygon.points[0].y = 1*math.sin(t)
    t = t + 0.1
    
    pub.publish(obstacle_msg)
    
    r.sleep()

if __name__ == '__main__': 
  try:
    publish_obstacle_msg()
  except rospy.ROSInterruptException:
    pass
}}}


Here we skip breaking the code down since creating a publisher and publishing messages
is explained well in the beginner tutorials section (see [[ROS/Tutorials/WritingPublisherSubscriber(python)|here]]). 

Now run the teb_local_planner `test_optim_node` in combination with rviz:
{{{
roslaunch teb_local_planner test_optim_node.launch
}}}

In the following, we assume that your package containing the python script is called `mypublisher`.
Make sure that your script is marked as executable (`chmod +x publish_obstacles.py`).
Open a new terminal and run your newly created python script:
{{{
roslaunch mypublisher publish_obstacles.py
}}}


The visualization in rviz should now look similar to the one shown in the following figure:
{{attachment:rviz_custom_obstacles_teb.png|Visualization: Taking custom obstacles into account|width="750"}}


== Related Parameters ==

Parameters that influence planning with custom obstacles are listed here (refer to the [[teb_local_planner|Node API]] for more details):
 *`~<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




## AUTOGENERATED DO NOT DELETE
## TutorialCategory
## FILL IN THE STACK TUTORIAL CATEGORY HERE