## 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]]
## note.1= [[teb_local_planner/Tutorials/Incorporate customized Obstacles|Incorporate customized Obstacles]]
## descriptive title for the tutorial
## title = Incorporate dynamic obstacles
## multi-line description to be displayed in search 
## description = In this tutorial you will learn how to take dynamic 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/Track and include dynamic obstacles via costmap_converter|Track and include dynamic obstacles via costmap_converter]]
## next.1.link=
## what level user is this tutorial for 
## level= IntermediateCategory
## keywords = Trajectory Optimization Local Planner Navigation Dynamic Obstacles
####################################

<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

<<TableOfContents(4)>>

== Introduction ==

This tutorial demonstrates how dynamic obstacles can be included in the teb_local_planner.
 
In a previous tutorial you learned how to provide user-supplied obstacles via an array of <<MsgLink(costmap_converter/ObstacleMsg)>> messages.
The message definition includes a <<MsgLink(geometry_msgs/TwistWithCovariance)>> field called `velocities` which captures the current centroid's velocity.
'''Currently, only `linear.x` and `linear.y` are processed by the teb_local_planner package.'''


== Write a Simple Dynamic Obstacle Publisher ==

In the following, we will create a small python node that publishes a dynamic obstacle.
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_dynamic_obstacles.py`:

{{{
#!python 
#!/usr/bin/env python

import rospy, math, tf
from costmap_converter.msg import ObstacleArrayMsg, ObstacleMsg
from geometry_msgs.msg import Point32, QuaternionStamped, Quaternion, TwistWithCovariance
from tf.transformations import quaternion_from_euler


def publish_obstacle_msg():
  pub = rospy.Publisher('/test_optim_node/obstacles', ObstacleArrayMsg, queue_size=1)
  rospy.init_node("test_obstacle_msg")

  y_0 = -3.0
  vel_x = 0.0
  vel_y = 0.3
  range_y = 6.0

  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 = 99
  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

  yaw = math.atan2(vel_y, vel_x)
  q = tf.transformations.quaternion_from_euler(0,0,yaw)
  obstacle_msg.obstacles[0].orientation = Quaternion(*q)

  obstacle_msg.obstacles[0].velocities.twist.linear.x = vel_x
  obstacle_msg.obstacles[0].velocities.twist.linear.y = vel_y
  obstacle_msg.obstacles[0].velocities.twist.linear.z = 0
  obstacle_msg.obstacles[0].velocities.twist.angular.x = 0
  obstacle_msg.obstacles[0].velocities.twist.angular.y = 0
  obstacle_msg.obstacles[0].velocities.twist.angular.z = 0

  r = rospy.Rate(10) # 10hz
  t = 0.0
  while not rospy.is_shutdown():
    
    # Vary y component of the point obstacle
    if (vel_y >= 0):
      obstacle_msg.obstacles[0].polygon.points[0].y = y_0 + (vel_y*t)%range_y
    else:
      obstacle_msg.obstacles[0].polygon.points[0].y = y_0 + (vel_y*t)%range_y - range_y

    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_dynamic_obstacles.py`).
Open a new terminal and run your newly created python script:
{{{
rosrun mypublisher publish_dynamic_obstacles.py
}}}

You should now see a visualization (rviz) that should look similar to the one in the previous tutorials, in particular, you should be able to see the default test_optim_node obstacles (interactive markers) and the new moving (dynamic) obstacle

However, the dynamic obstacle feature is not yet enabled in the teb_local_planner/test_optim_node. We will do this in the next section.

Before you proceed, please move the interactive markers far away from the planning scene, like in the following figure:

{{attachment:dynamic_obstacle_rviz_deactivated.png|Visualization: The dynamic obstacle position is published by the above python script. Interactive markers are moved away. Parameter include_dynamic_obstacles is disabled.|width="750"}}

== Set up the Planner for Considering Dynamic Obstacles ==

Start the `rqt_reconfigure` node in a new terminal:
{{{
rosrun rqt_reconfigure rqt_reconfigure
}}}

Now activate the check-box for parameter `include_dynamic_obstacles` (obstacles section).
The teb_local_planner now utilizes a constant-velocity-model to predict the obstacles future motion. You should now recognize that the ego-trajectory avoids the obstacle based on the spatio-temporal distance rather than based on the current obstacle position (see the next figure).
Note, it is important that the velocity estimation is accurate and that the constant-velocity assumption is fairly satisfied.

{{attachment:dynamic_obstacle_rviz_activated.png|Visualization: The dynamic obstacle position is published by the above python script. Interactive markers are moved away. Parameter include_dynamic_obstacles is activated.|width="750"}}

You can also visualize the temporal evolution of the planned and predicted velocities if you adjust parameter `visualize_with_time_as_z_axis`. You can set the parameter value now to `0.1`.
The z-axis in rviz is now interpreted as time-axis scaled by the specified factor.
Here you can also see how the homotopy-class-planning (planning in distinctive topologies) takes the dynamic obstacle predictions into account:

{{attachment:dynamic_obstacle_rviz_z_scaled.png|The temporal information of the trajectory and predictions is now visualized w.r.t. the z-axis|width="750"}}

You might have noticed, that two of the interactive marker obstacles of the `test_optim_node` are defined with a non-zero velocity. Feel free to move them into the planning scene and observe how the planner performs:

{{attachment:dynamic_obstacle_rviz_z_scaled2.png|The temporal information of the trajectory and predictions is now visualized w.r.t. the z-axis|width="750"}}

Note, the teb_local_planner defines some dedicated optimization weights for dynamic obstacles. Refer to the list in the next section.


== 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 (static and dynamic) obstacles
 *`~<name>/inflation_dist`: Non-zero cost region around (static) obstacles
 *`~<name>/include_dynamic_obstacles`: Specify whether the motion of dynamic obstacles should be included (constant-velocity-model) or not.
 *`~<name>/dynamic_obstacle_inflation_dist`: Non-zero cost region around (dynamic) 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 static obstacles.
 *`~<name>/weight_inflation`: Optimization weight for inflation costs of static obstacles.
 *`~<name>/weight_dynamic_obstacle`: Optimization weight for keeping a distance to dynamic obstacles.
 *`~<name>/weight_dynamic_obstacle_inflation`: Optimization weight for inflation costs of dynamic obstacles.
 *`~<name>/footprint_model`: The robot footprint model


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