===== P2 - Multi-goal Inspection Planning ======
The main task is to implement ROS modules for multi-goal multi-robot inspection planning based on the task [[courses:b4m36uir:hw:t2b-dtspn|T2b-dtspn]].

|**Deadline**  |  January 5th, 23:59 PST |
|**Points**  |  10 |
|**Label in BRUTE**  |  P2 |
|**Evaluation**  | Solution is evaluated by a demonstration to an instructor |
|**Resources**  |  {{:courses:b4m36uir:projects:uir-p2-data.zip | P2-data resource package}} |

\\

===Assignment===
Implement ROS nodes to solve the multi-goal multi-robot inspection planning task.
The provided resource pack supports deployment using the STDR simulator.
Moreover, an R-VIZ setup file is provided to visualize the robot knowledge of the environment using the messages published by the individual ROS nodes.

\\

==Robot Motion Constraints==
The robot is constrained by its minimal velocity $$v_{min} = 1 \text{ ms}^{-1}$$ and minimal turning radius $$r_{min} = 0.5 \text{ m}.$$



\\

==Simulator==

The simulator provides and processes the following inputs and outputs, respectively

|        | Message type | ROS Topic | Description |
| Outputs| Robot pose ([[http://docs.ros.org/melodic/api/nav_msgs/html/msg/Odometry.html|nav_msgs/Odometry]]) | /robot{i}/odom | Robot pose provided by a simulator |
|        | Point cloud ([[http://docs.ros.org/melodic/api/sensor_msgs/html/msg/PointCloud2.html|sensor_msgs/PointCloud2]] ) | /goals | Goals to be visited by the robots, where each point comprises the ''x'', ''y'', and ''radius'' fields |
| Inputs | Velocity command ([[https://docs.ros.org/api/geometry_msgs/html/msg/Twist.html|geometry_msgs/Twist]] ) | /robot{i}/cmd_vel/dubins | Velocities to drive the robot which are compliant to the Dubins vehicle model constraints, as specified in the Robot Motion Constraints section |

subject to
$$i \in {0,1,...,N-1}$$
where ''N'' is number of robots in the simulation. Thus, the velocity command topic for the first robot is ''/robot0/cmd_vel/dubins''.

\\

To run the STDR simulator
    - launch the ROS node
<code bash>
roslaunch uir_multi_goal_tools uir_backend_multi_goal.launch [robot_n:=number_of_robots]
</code>
where the optional argument ''robot_n'' specifies the number of robots in the simulation.

\\

==RVIZ==

The provided RVIZ setup file may be used to visualize the following topics

| Message type | Topic | Description |
| Robot odometry ([[http://docs.ros.org/melodic/api/nav_msgs/html/msg/Odometry.html|nav_msgs/Odometry]]) | /robot{i}/odom | Robot position and orientation provided by a simulator |
| Path ([[http://docs.ros.org/melodic/api/nav_msgs/html/msg/Path.html|nav_msgs/Path]]) | /robot{i}/path | Dubins path to be followed by the robot |
| Occupancy map ([[http://docs.ros.org/melodic/api/nav_msgs/html/msg/OccupancyGrid.html|nav_msgs/OccupancyGrid]] ) | /gridmap | Map of the robot surroundings (published by the simulation backend) |
| Marker array ([[http://docs.ros.org/melodic/api/visualization_msgs/html/msg/MarkerArray.html|visualization_msgs/MarkerArray]] ) | /goals/neighborhoods | Goal neighborhoods (published by the simulation backend) |

\\

== Expected Behavior ==
{{:courses:b4m36uir:projects:p2.gif?300|}}

\\

=== Evaluation ===
A demonstration in STDR is evaluated by an instructor during the labs. 

\\

===Suggested architecture===

\\

==Path following module==
The path following module drives a curvature constrained robot along a given path by publishing velocity commands.
The followed path represents a tour of goal locations and should be followed indefinitely.
Moreover, an individual instance of this node should be created for each of the robots. 

|        | Message type | Topic | Description 
| Inputs | Path ([[http://docs.ros.org/melodic/api/nav_msgs/html/msg/Path.html|nav_msgs/Path]]) | /robot{i}/path | Dubins path to be followed by the robot |
|        | Robot odometry ([[http://docs.ros.org/melodic/api/nav_msgs/html/msg/Odometry.html|nav_msgs/Odometry]]) | /robot{i}/odom | Robot position and orientation provided by a simulator |
| Outputs | Velocity command ([[https://docs.ros.org/api/geometry_msgs/html/msg/Twist.html|geometry_msgs/Twist]] ) | /robot{i}/cmd_vel/dubins | Velocities to drive the robot which are compliant to the Dubins vehicle model constraints, as specified in the Robot Motion Constraints section |

Note, commands that do not comply the robot motion constraints should be avoided, as they may lead to unpredictable behavior.
\\
Assume that the command velocity issued as
$$
\begin{aligned}
&\text{linear}:\\
&\quad x: 1.0\\
&\quad y: 0.0\\
&\quad z: 0.0\\
&\text{angular}:\\ 
&\quad x: 0.0\\
&\quad y: 0.0\\
&\quad z: 2.0\\
\end{aligned}
$$
corresponds to the robot moving with the forward velocity
$$v = 1 \text{ ms}^{-1}$$
and turning with the turning radius
$$r = 0.5 \text{ m}.$$

\\  
  
==Goal clustering module==
The goal clustering module assigns goals to the individual robots.

|         | Message type | Topic | Description 
| Inputs  | Point cloud ([[http://docs.ros.org/melodic/api/sensor_msgs/html/msg/PointCloud2.html|sensor_msgs/PointCloud2]] ) | /goals | Goals to be visited by the robots, where each point comprises the ''x'', ''y'', and ''radius'' fields |
|         | Robot odometry ([[http://docs.ros.org/melodic/api/nav_msgs/html/msg/Odometry.html|nav_msgs/Odometry]]) | /robot{i}/odom | Robot position and orientation provided by a simulator | 
| Outputs |Point cloud ([[http://docs.ros.org/melodic/api/sensor_msgs/html/msg/PointCloud2.html|sensor_msgs/PointCloud2]] ) | /robot{i}/goals | Goals to be visited by a robot, where each point comprises the ''x'', ''y'', and ''radius'' fields |

\\

==Path planning module==
The path planning module plans a closed loop path over goals assigned to a robot.

|         | Message type | Topic | Description 
| Inputs  | Robot pose ([[http://docs.ros.org/melodic/api/nav_msgs/html/msg/Odometry.html|nav_msgs/Odometry]]) | /robot{i}/odom | Robot pose provided by a simulator |
|         |Point cloud ([[http://docs.ros.org/melodic/api/sensor_msgs/html/msg/PointCloud2.html|sensor_msgs/PointCloud2]] ) | /robot{i}/goals | Goals to be visited by a robot, where each point comprises the ''x'', ''y'', and ''radius'' fields |
| Outputs | Path ([[http://docs.ros.org/melodic/api/nav_msgs/html/msg/Path.html|nav_msgs/Path]]) | /robot{i}/path | Path followed by the robot |

Use either the decoupled or Noon-Bean approach, or an appropriate combination based on the goals assigned to the robot. 

\\

===Appendix===

\\

==Setup and ROS Basics==

To run the ROS nodes, it is strongly recommended to use either Ubuntu 16 with ROS Kinetic or Ubuntu 18 with ROS Melodic.
Detailed installation instructions may be found on ROS web pages
[[http://wiki.ros.org/kinetic/Installation/Ubuntu|ROS Kinetic]], [[http://wiki.ros.org/melodic/Installation/Ubuntu|ROS Melodic]].

On the lab computers, ROS Kinetic is already preinstalled but two more libraries need to be installed before using it.
<code bash>

pip install rospkg
pip install netifaces

</code>
Moreover, it is necessary to source the main ROS setup script, which has to be done in each new terminal instance.
Thus, it is recommended to add the following source command in your ''~/.bashrc'' file
<code bash>
echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc
</code>
or alternatively when running ROS Meloding
<code bash>
echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc
</code>

Finally, on Ubuntu 16 the STDR simulator is installed using
<code bash>
sudo apt-get install ros-kinetic-stdr-*
</code>
On Ubuntu 18, STDR has to be compiled from source, which can be found at [[https://github.com/stdr-simulator-ros-pkg/stdr_simulator]].

\\

==ROS Basics==
On Ubuntu 16 and 18 the ROS workspace is created as follows
<code bash>

cd ~/
mkdir rds_ws
mkdir rds_ws/src
cd rds_ws
catkin_make

# source the workspace
cd devel
devel_path=$(pwd)
echo "source $devel_path/setup.bash" >> ~/.bashrc
source setup.bash

# Add new (custom) package to the workspace
cd ~/rds_ws/src                 # Go to src folder in your workspace.
ln -s [path to your package]    # Create symlink to your package (or place the whole package directly to src).
cd ~/rds_ws
catkin_make                     # Compile the workspace.

rospack list                    # Make sure that your pack is known. (Sometimes this is necessary to refresh ROS. ) 
                                # Now, you should be able to use the package within ROS (rosrun or roslaunch).

# useful commands
rostopic list              # all current topics in the ROS system
rostopic info [topic name] # show info about topic - publishers, subscribers
rostopic echo [topic name] # writes msgs data on certain topic to stdout
rostopic hz [topic name]   # prints frequency of msgs on a certain topic

rosnode list               # all currently running nodes
rosnode info [node name]   # info about the node: published topics, subscribed topics

# useful tools
rqt_graph                  # visualize connection between the nodes (might not be 100% accurate)
rviz                       # spatial visualization of the msgs
rosrun tf view_frames      # creates pdf with a visualization of the whole transformation tree (all the /tf msgs in the system) 
rosbag record              # capture ROS msgs on selected topics (data can be played with selected speed afterwards) 
rosbag play [bagfile]      # play data saved in bagfile

</code>

The main study material for ROS is [[http://wiki.ros.org/ROS/Tutorials]].
A special attention should be given to ROS message-based communication presented in [[http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28python%29]].