# Robossembler Plansys2 example

Plansys2 & MoveIt2 example on ROS2 Foxy

### Install
Install ```colcon``` with ```mixin``` (it's required to install ```moveit_visual_tools```):
```bash
sudo apt install python3-colcon-common-extensions python3-colcon-mixin python3-vcstool
colcon mixin add default https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml
colcon mixin update default
```

Prepare workspace & install dependencies
```
mkdir -p ~/robossembler_ws/src && cd ~/robossembler_ws/src
git clone https://gitlab.com/robosphere/robossembler-ros2 -b dev-motion-planning-plansys2
vcs import . < robossembler-ros2/rasms.repos
cd ..
rosdep install -y -r -q --from-paths src --ignore-src --rosdistro foxy
colcon build --symlink-install --mixin release
```

### Run
Add source to environment
```
source install/setup.bash
```
Launch MoveIt2, Gazebo, RViz
```bash
ros2 launch rasms_moveit_config rasms_bringup.launch.py
```
Launch PlanSys2 with domain from ```pddl/domain.pddl```
```bash
ros2 launch rasms_manipulator rasms_manipulation.launch.py
```
Launch Plansys2 Terminal
```bash
ros2 run plansys2_terminal plansys2_terminal
```
Then into plansys2_terminal paste command
```bash
set instance rasms robot
set instance one zone
set predicate (robot_at rasms one)
set goal (and(robot_moved rasms one))
run
```

### Links
* [plansys2_bt](https://intelligentroboticslab.gsyc.urjc.es/ros2_planning_system.github.io/tutorials/docs/bt_actions.html)
* [bt_v3_cpp](https://www.behaviortree.dev)
* [moveit2](https://moveit.picknik.ai/foxy/index.html)