# Robossembler ROS2

Repo for ROS2 packages related to Robossembler

## Instructions
### Requirements
* OS: Ubuntu 22.04
* ROS 2 Humble

### Dependencies
These are the primary dependencies required to use this project.

* MoveIt 2 
    > Install/build a version based on the selected ROS 2 release
* Gazebo Fortress

### Build

1. Clone the repository
2. Build packages `colcon build`


Prepare workspace & install dependencies (So far only tested with UR robot arm)
```bash
mkdir -p ~/robossembler_ws/src && cd ~/robossembler_ws/src
git clone https://gitlab.com/robosphere/robossembler-ros2
vcs import . < robossembler-ros2/rbs.repos
cd ..
rosdep install -y -r -q --from-paths src --ignore-src --rosdistro humble
colcon build --packages-skip cartesian_controller_simulation cartesian_controller_tests --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install
```
For simulation with Mujoco, pls [ref](https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers/blob/fdaa9a2dd0ce15fc949b127dd1164095504273fb/cartesian_controller_simulation/README.md) build and install section.

### Set Gazebo enviroment variables
Replace `[WS_FOLDER]` with your workspace folder
```bash
echo "export IGN_GAZEBO_RESOURCE_PATH=${IGN_GAZEBO_RESOURCE_PATH}:~/[WS_FOLDER]/install/robotiq_description/share/:~/[WS_FOLDER]/install/rbs_simulation/share/rbs_simulation/" >> ~/.bashrc
```


### Examples
Activate current ROS2 enviroment:
```
. install/setup.bash
```

Launch MoveIt2, Gazebo, RViz:
```bash
ros2 launch rbs_bringup bringup.launch.py
```

Load Problem file via ROS2 Service (TMP):
```
ros2 service call /problem_expert/add_problem plansys2_msgs/srv/AddProblem '{problem: (define (problem box_problem)(:domain atomic_domain)(:objects ur_manipulator_gripper - arm workspace1 - zone box1 box2 box3 - part asm0 asm1 asm2 asm3 - assembly)(:init (part_of box1 asm1)(part_of box2 asm2)(part_of box3 asm3)(assembly_order asm0 asm1)(assembly_order asm1 asm2)(assembly_order asm2 asm3)(arm_available ur_manipulator_gripper)(assembly_at asm0 workspace1)(assembled asm0))(:goal (and (assembled asm3))))}'
```

Start task planner 
```
ros2 run rbs_task_planner execute_plan 
```

### Links
* [bt_v3_cpp](https://www.behaviortree.dev)
* [moveit2](https://moveit.picknik.ai/humble/index.html)