Merge branch 'GEAR'

.gitignore

@@ -0,0 +1,37 @@
+# Don't track the content of these folders #
+############################################
+src/*/lib/
+src/*/build
+src/*/devel
+src/*/install
+
+# Don't track catkin build files #
+##################################
+core
+
+# vscode and other system specific files #
+##########################################
+.*
+
+# Include gitignore #
+#####################
+!.gitignore
+
+# Compiled source #
+###################
+*.o
+*.so
+
+# Packages #
+############
+*.tar
+*.gz
+*.zip
+
+# Other #
+#########
+*.backup
+*.old
+*.custom
+*.run
+*.debug

docker_env/docker-compose.yml

@@ -3,6 +3,10 @@
 # docker-compose.yml format version
 version: '3'
 
+networks:
+    ros:
+        driver: bridge
+
 # Define services
 services:
     # ROS Development Service
@@ -55,10 +59,30 @@ services:
         build: ./gazebo
         # Mount gazebo folder on host to app folder in container
         volumes:
-            - ./gazebo/.gazebo:/root/.gazebo
+            #- .docker_env/gazebo/.gazebo:/root/.gazebo
+            - ../src/rrrobot_ws:/app/rrrobot_ws
+        # Set DISPLAY variable and network mode for GUIs
+        environment:
+            - DISPLAY=${IP_ADDRESS}:0.0
+        # network_mode: "host"
+        # Set working directory in container to app folder
+        #working_dir: /home/rrrobot
+        hostname: "rrrobot-env"
+        networks:
+            - ros
+    
+    # GEAR Production Service
+    gear:
+        # Use Dockerfile in gear folder
+        build: ./gear
+        # Mount gear folder on host to app folder in container
+        volumes:
+            - ../src/rrrobot_ws:/app/rrrobot_ws
         # Set DISPLAY variable and network mode for GUIs
         environment:
             - DISPLAY=${IP_ADDRESS}:0.0
-        network_mode: "host"
         # Set working directory in container to app folder
         working_dir: /app
+        hostname: "rrrobot-env"
+        networks:
+            - ros

docker_env/gazebo/Dockerfile

@@ -0,0 +1,40 @@
+# Gazebo Dockerfile
+
+# Use official image for Gazebo 9.x 
+FROM gazebo:gzserver9
+
+RUN apt-get update
+# Install packages required for developing with gazebo
+RUN apt-get install -y libgazebo9-dev
+RUN sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
+RUN apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
+RUN apt-get install -y curl
+RUN curl -sSL 'http://keyserver.ubuntu.com/pks/lookup?op=get&search=0xC1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' | apt-key add -
+RUN apt-get update
+# Install packages required for developing with ROS
+RUN apt-get install -y ros-melodic-desktop-full
+RUN echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc
+RUN apt install -y python-rosdep
+RUN rosdep init
+RUN rosdep update
+RUN apt-get install -y ros-melodic-pid
+# user id 1000 should be the same as the host user, so that you can access files
+# from inside the docker container and also on the host
+RUN useradd -u 1000 rrrobot
+# set to no password
+RUN passwd --delete rrrobot
+# add to sudo users
+RUN usermod -aG sudo rrrobot
+# set the entry point
+WORKDIR /home/rrrobot
+RUN chown -R rrrobot:rrrobot /home/rrrobot
+
+# Initialize the environment in .bashrc
+RUN echo "source /opt/ros/melodic/setup.bash" >> /home/rrrobot/.bashrc
+RUN echo "source /usr/share/gazebo/setup.sh" >> /home/rrrobot/.bashrc
+RUN echo "export GAZEBO_MODEL_PATH=/home/rrrobot/rrrobot_src/src/gazebo_models:\$GAZEBO_MODEL_PATH" >> /home/rrrobot/.bashrc
+RUN echo "export GAZEBO_PLUGIN_PATH=/home/rrrobot/rrrobot_src/lib:\$GAZEBO_PLUGIN_PATH" >> /home/rrrobot/.bashrc
+
+USER rrrobot
+
+CMD ["/bin/bash"]

docker_env/gazebo/build.sh

@@ -0,0 +1 @@
+docker image build . -t eecs467:rrrobot

docker_env/gazebo/run_rrrobot_image.sh

@@ -0,0 +1 @@
+../../run_rrrobot_image.sh

docker_env/gear/Dockerfile

@@ -0,0 +1,67 @@
+# GEAR Dockerfile
+
+# Use official image for Gazebo 9.x 
+FROM gazebo:gzserver9
+
+RUN apt-get update
+# Install packages required for developing with gazebo
+RUN apt-get install -y libgazebo9-dev
+RUN sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
+RUN apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
+RUN apt-get install -y curl
+RUN curl -sSL 'http://keyserver.ubuntu.com/pks/lookup?op=get&search=0xC1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' | apt-key add -
+RUN apt-get update
+# Install packages required for developing with ROS
+RUN apt-get install -y ros-melodic-desktop-full
+RUN echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc
+RUN apt install -y python-rosdep
+RUN rosdep init
+RUN rosdep update
+# user id 1000 should be the same as the host user, so that you can access files
+# from inside the docker container and also on the host
+RUN useradd -u 1000 rrrobot
+# set to no password
+RUN passwd --delete rrrobot
+# add to sudo users
+RUN usermod -aG sudo rrrobot
+# set the entry point
+WORKDIR /app/rrrobot_ws
+RUN mkdir /home/rrrobot
+RUN chown -R rrrobot:rrrobot /home/rrrobot
+
+# Initialize the environment in .bashrc
+RUN echo "source /opt/ros/melodic/setup.bash" >> /home/rrrobot/.bashrc
+RUN echo "source /usr/share/gazebo/setup.sh" >> /home/rrrobot/.bashrc
+RUN echo "export GAZEBO_MODEL_PATH=/app/rrrobot_ws/src/gazebo_models:\$GAZEBO_MODEL_PATH" >> /home/rrrobot/.bashrc
+RUN echo "export GAZEBO_PLUGIN_PATH=/opt/ros/melodic/lib:/app/rrrobot_ws/devel/lib:\$GAZEBO_PLUGIN_PATH" >> /home/rrrobot/.bashrc
+
+USER rrrobot
+
+# Install Gazebo Environment for Agile Robotics (GEAR)
+RUN sudo apt-get update && \
+    sudo apt-get install -y \
+        wget
+
+RUN sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable bionic main" > /etc/apt/sources.list.d/gazebo-stable.list' && \
+    wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add - && \
+    sudo apt-get update && \
+    sudo apt-get install -y \
+        ariac3
+
+# CV Model Dependencies
+RUN sudo apt-get update && \
+    sudo apt-get install -y \
+        python3-pip
+
+RUN pip3 install -U \
+        numpy \
+        torch \
+        torchvision \
+        Pillow
+
+RUN pip3 install pyyaml
+RUN pip3 install rospkg
+RUN pip3 install matplotlib
+RUN sudo apt-get install python3-tk
+
+CMD ["/bin/bash"]

docker_env/ros-dev/build.sh

@@ -0,0 +1 @@
+docker image build . --rm -t eecs467:rrrobot

docker_env/ros-dev/run_rrrobot_image.sh

@@ -0,0 +1 @@
+../run_rrrobot_image.sh

docker_env/ros-turtlesim/build.sh

@@ -0,0 +1 @@
+docker image build . --rm -t eecs467:rrrobot

docker_env/ros-turtlesim/run_rrrobot_image.sh

@@ -0,0 +1,5 @@
+#docker run -i -h rrrobot-env -t eecs467:rrrobot bash
+
+xhost +local:docker #rrrobot-env
+docker run -it --rm --privileged --device=/dev/dri:/dev/dri -e DISPLAY=$DISPLAY -v $PWD:/app -v /tmp/.X11-unix:/tmp/.X11-unix -h rrrobot-env eecs467:rrrobot
+xhost -local:docker #rrrobot-env

docker_env/ros/build.sh

@@ -0,0 +1 @@
+docker image build . --rm -t eecs467:rrrobot

docker_env/ros/run_rrrobot_image.sh

@@ -0,0 +1,5 @@
+#docker run -i -h rrrobot-env -t eecs467:rrrobot bash
+
+#docker run -it --rm --security-opt seccomp=unconfined -v $SSH_AUTH_SOCK:/ssh-agent --env SSH_AUTH_SOCK=/ssh-agent --device=/dev/dri:/dev/dri -e DISPLAY=$DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix --env QT_X11_NO_MITSHM=1 -h rrrobot-env eecs467:rrrobot
+
+../run_rrrobot_image.sh

docs/documentation.md

@@ -0,0 +1,78 @@
+# Documentation <!-- omit in toc -->
+
+## Page Links <!-- omit in toc -->
+- [Home](home.md)
+- [Documentation](documentation.md)
+- [GEAR](gear.md)
+
+## Table of Contents <!-- omit in toc -->
+- [Directory Structure](#directory-structure)
+  - [docker_env](#dockerenv)
+  - [docs](#docs)
+  - [src](#src)
+    - [src/rrrobot_ws/src/gazebo_models](#srcrrrobotwssrcgazebomodels)
+    - [src/rrrobot_ws/src/rrrobot](#srcrrrobotwssrcrrrobot)
+    - [src/rrrobot_ws/world](#srcrrrobotwsworld)
+  - [utils](#utils)
+
+## Directory Structure
+
+In the RRRobot folder of our repository we have the following folders.
+
+### docker_env
+
+This contains the [docker-compose.yml](https://github.com/EECS-467-W20-RRRobot-Project/RRRobot/blob/master/docker_env/docker-compose.yml) file and folders for each docker service described in the [home](home.md) page. **gear** is the docker image used for our final simulation.
+
+### docs
+
+This contains the markdown files used to generate the project website pages and project documents like the poster and proposal.
+
+### src
+
+This contains the rrrobot_ws catkin workspace for our ROS development. Inside the `rrrobot_ws` folder is a [run_all.sh](https://github.com/EECS-467-W20-RRRobot-Project/RRRobot/blob/master/src/rrrobot_ws/run_all.sh) that will run the final simulation. See the [GEAR](gear.md) page for full instructions on using this.
+
+In `rrrobot_ws/src`, we have a `gazebo_models` and `rrrobot` folder.
+
+#### src/rrrobot_ws/src/gazebo_models
+
+`gazebo_models`, as the name implies, contains 3D models we used in our testing. These include a fanuc robot arm with gripper, paper bag, plastic bottle, pop can, etc. For our final simulation, we decided to use simple cubes to represent trash (red) and recycling (green) items to make the ur10 arm's vacuum suction gripper work well. We also have the images that are passed to the CV model for classification in the `recycling_images` and `trash_images` folders. `model_mappings.txt` defines the model to use, location of image, and ground truth classification in a comma separated format.
+
+#### src/rrrobot_ws/src/rrrobot
+
+Our ROS package contains several folders.
+
+`config` contains the configuration `.yaml` files that we use to overwrite ARIAC's default configuration. These files remove a bunch of the parts that we don't need and specify that we only need 1 depth camera.
+
+`include` contains the header files for the names of topics that are used by our ROS nodes and the class definition for `ArmRepresentation`.
+
+`launch` contains the launch file that we use to overwrite ARIAC's default launch file.
+
+`msg` contains the definitions for the custom ROS messages used by our nodes.
+
+`scripts` contains the bash scripts we used throughout testing. These include sending the arm to the home position, publishing a test message for the arm controller to listen to, and running the modified version of the GEAR simulation environment.
+
+`src` contains the source code for our ROS nodes.
+
+- `arm_controller_node.cpp` receives a pose from rrrobot_node to pick up the object, then uses inverse kinematics to determine the joint positions that will achieve the desired end-effector pose.
+- `arm_representation_node.cpp` interfaces with [KDL](http://docs.ros.org/melodic/api/orocos_kdl/html/index.html) to perform inverse and forward kinematics on our ur10 robot arm model.
+- `cv_model.py` uses `pytorch_pretrain_model.pt` to classify images that it receives and send the classification to rrrobot_node.
+- `depth_camera_node.cpp` takes depth camera information and determines an end-effector pose for the robot arm to reach so it can pick up the object.
+- `image_display.py` is a test script that uses [matplotlib](https://matplotlib.org/) to display images.
+- `model_insertion_plugin.cpp` and `object_spawner_node.cpp` interface with gazebo to spawn the items in the simulation so the arm can sort them.
+- `rrrobot_node.cpp` is the main node that handles interactions between the different nodes.
+
+`test` contains some ROS nodes that we created to test forward and inverse kinematics.
+
+`CMakeLists.txt` is used by our build system, [CMake](https://cmake.org), to build and compile our ROS package.
+
+`package.xml` defines the metadata for our ROS package.
+
+#### src/rrrobot_ws/world
+
+This folder contains the world files that we use to overwrite ARIAC's defaults.
+
+In `gear.py` we have disabled all but two of the bins as we only need to classify recycling and trash.
+
+### utils
+
+This contains config.xlaunch which is used by VcXsrv to automatically start the server with the desired settings (see [home](home.md) page for more information).

docs/gear.md

@@ -0,0 +1,152 @@
+# GEAR Simulation <!-- omit in toc -->
+
+## Page Links <!-- omit in toc -->
+- [Home](home.md)
+- [Documentation](documentation.md)
+- [GEAR](gear.md)
+
+## Table of Contents <!-- omit in toc -->
+- [Running GEAR Container](#running-gear-container)
+- [Building & Running Simulation](#building--running-simulation)
+- [Controlling Sample Environment](#controlling-sample-environment)
+  - [Start Competition](#start-competition)
+  - [Controlling Arms](#controlling-arms)
+- [Running Full Simulation](#running-full-simulation)
+  - [Manual Process for Testing](#manual-process-for-testing)
+  - [Semi-Automated Process](#semi-automated-process)
+
+## Running GEAR Container
+
+1. On your local system, go to `docker_env` folder
+   - `cd /PATH/TO/RRRobot/docker_env`
+2. Start GEAR docker container
+   - `docker-compose run --rm gear`
+
+## Building & Running Simulation
+
+1. Source ROS Setup
+   - `source /opt/ros/melodic/setup.bash`
+2. Run Simulation
+   - Sample Environment
+     - `roslaunch osrf_gear sample_environment.launch`
+     - See [controlling sample environment](#controlling-sample-environment) for more information.
+   - Final Simulation
+     - See [running full simulation](#running-full-simulation) for more information.
+
+
+## Controlling Sample Environment
+
+While sample environment is running, open a new terminal (see tips & tricks section of [home](home.md)) on the running docker container. The following sections provide a brief overview of the commands that can be used to control various aspects of the simulation. For a full list, check out the [ARIAC 2019 Wiki](https://bitbucket.org/osrf/ariac/wiki/2019/tutorials/gear_interface).
+
+[sample_run.sh](https://github.com/EECS-467-W20-RRRobot-Project/RRRobot/blob/master/src/rrrobot_ws/src/rrrobot/scripts/sample_run.sh) moves arm 1 over the gasket part, picks it up, moves it to AGV1's tray, drops it, and moves back to the starting position.
+
+### Start Competition
+
+`rosservice call /ariac/start_competition`
+
+### Controlling Arms
+
+#### Gripper <!-- omit in toc -->
+
+- Turn gripper suction on
+  - `rosservice call /ariac/arm1/gripper/control "enable: true"`
+- Turn gripper suction off
+  - `rosservice call /ariac/arm1/gripper/control "enable: false"`
+
+#### Joints <!-- omit in toc -->
+
+Move `arm1` over a gasket part
+
+```
+rostopic pub /ariac/arm1/arm/command trajectory_msgs/JointTrajectory    "{joint_names: \
+        ['linear_arm_actuator_joint',  'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'], \
+     points: [ \
+{time_from_start: {secs: 2}, \
+        positions: [0.15, 3.14,  -1.570,  2.14, 3.1, -1.59, 0.126]}, \
+{time_from_start: {secs: 4}, \
+        positions: [-0.35, 3.14,  -0.6,  2.3, 3.0, -1.59, 0.126]}, \
+{time_from_start: {secs: 6}, \
+        positions: [-0.35, 3.14,  -0.5,  2.3, 3.05, -1.59, 0.126]}, \
+]}" -1
+```
+
+Move part to `AGV1`'s tray
+
+```
+rostopic pub /ariac/arm1/arm/command trajectory_msgs/JointTrajectory    "{joint_names: \
+        ['linear_arm_actuator_joint',  'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'], \
+     points: [ \
+{time_from_start: {secs: 2}, \
+        positions: [0.0, 3.14,  -1.570,  2.14, 3.27, -1.51, 0.0]}, \
+{time_from_start: {secs: 5}, \
+        positions: [1.0, 1.85,  0,  -0.38, 1.57, -1.51, 0.00]}, \
+{time_from_start: {secs: 7}, \
+        positions: [1.0, 1.507,  0,  -0.38, 0.38, -1.51, 0.00]}, \
+{time_from_start: {secs: 10}, \
+        positions: [1.18, 1.507,  0.38,  -0.38, 1.55, 1.75, 0.127]}, \
+]}" -1
+```
+
+Return to starting position
+
+```
+rostopic pub /ariac/arm1/arm/command trajectory_msgs/JointTrajectory    "{joint_names: \
+        ['linear_arm_actuator_joint',  'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'], \
+     points: [ \
+{time_from_start: {secs: 5}, \
+        positions: [0.0, 3.14,  -1.570,  2.14, 3.27, -1.51, 0.0]}, \
+]}" -1
+```
+
+## Running Full Simulation
+
+### Manual Process for Testing
+
+#### Terminal 1: ARIAC Environment <!-- omit in toc -->
+
+- `cd /app/rrrobot_ws/src/rrrobot/scripts`
+- `./rrrobot_run_no_build.sh`
+
+#### Terminal 2: Build RRRobot Package & Arm Controller Node <!-- omit in toc -->
+
+- `cd /app/rrrobot_ws`
+- `catkin_make clean`
+- `catkin_make`
+- `source devel/setup.bash`
+- `rosrun rrrobot arm_controller_node`
+
+#### Terminal 3: CV Model <!-- omit in toc -->
+
+- `cd /app/rrrobot_ws/src/rrrobot/src/`
+- `python3 cv_model.py`
+
+#### Terminal 4: Depth Camera Node (for getting pick up location) <!-- omit in toc -->
+
+- `cd /app/rrrobot_ws/`
+- `source devel/setup.bash`
+- `rosrun rrrobot depth_camera_node`
+
+#### Terminal 5: Run the main rrrobot node <!-- omit in toc -->
+
+- `cd /app/rrrobot_ws/`
+- `source devel/setup.bash`
+- `rosrun rrrobot rrrobot_node`
+
+#### Terminal 6: Run the node to spawn random objects onto the conveyor belt <!-- omit in toc -->
+
+- `cd /app/rrrobot_ws/`
+- `source devel/setup.bash`
+- `rosrun rrrobot object_spawner_node`
+
+### Semi-Automated Process
+
+#### Terminal 1 <!-- omit in toc -->
+
+In terminal 1, run the script that will build all components and run the nodes. You may need to adjust the sleep times in [run_all.sh](https://github.com/EECS-467-W20-RRRobot-Project/RRRobot/blob/master/src/rrrobot_ws/run_all.sh) based on your system's performance such that there is enough time for each component to finish before proceeding.
+
+- `cd /app/rrrobot_ws/`
+- `./run_all.sh`
+
+#### Terminal 2 <!-- omit in toc -->
+
+When a new item is spawned after the arm drops off the previous item, the conveyor belt must be manually started again using `rosservice call /ariac/conveyor/control "power: 100"` so that the item moves to the depth camera and the process can continue. You can do this in a new terminal since terminal 1 is running all of the simulation nodes.

docs/home.md

@@ -1,5 +1,10 @@
 # RRRobot! <!-- omit in toc -->
 
+## Page Links <!-- omit in toc -->
+- [Home](home.md)
+- [Documentation](documentation.md)
+- [GEAR](gear.md)
+
 ## Table of Contents <!-- omit in toc -->
 - [Contributors](#contributors)
 - [Documents](#documents)
@@ -8,6 +13,10 @@
   - [Docker Compose Services](#docker-compose-services)
   - [GUI Support](#gui-support)
   - [Running Docker Containers](#running-docker-containers)
+  - [Gazebo Grasping Simulation](#gazebo-grasping-simulation)
+  - [Tips & Tricks](#tips--tricks)
+- [Source Code Documentation](#source-code-documentation)
+- [GEAR Final Simulation](#gear-final-simulation)
 
 ## Contributors
 
@@ -28,6 +37,8 @@
 
 1. [Project Poster](1.%20Project%20Poster.pdf)
 2. [Project Proposal](2.%20Project%20Proposal.pdf)
+3. [Final Project Report - Overleaf (Read-Only)](https://www.overleaf.com/read/ncvksrzpvbmr)
+4. [Draft Video](https://youtu.be/powEVDPQNEY)
 
 ## Introduction
 
@@ -35,6 +46,10 @@ Repository for UMICH EECS 467: Autonomous Robotics (W20) RRRobot! project.
 
 To get started, you will need to install Docker on your system. Information on what Docker is, how to install it, and how to use it can be found in [Getting Started with Docker](https://sravanbalaji.com/Web%20Pages/blog_docker.html).
 
+Once installed, you can run our final simulation by following the instructions on the [GEAR](gear.md) page.
+
+Information about our source code can be found on the [Documentation](documentation.md) page.
+
 ## Development Guide
 
 At this point, you should have Docker Desktop, Toolbox, or Engine setup on your host system. In this section, you will find a list of the different docker-compose services available and how to run them.
@@ -51,15 +66,19 @@ ros-turtlesim is a container that tests whether GUI support is working. This wil
 
 #### ros <!-- omit in toc -->
 
-ros is the production container for all ROS nodes we create. This is meant to run our final code and interface with the gazebo container for simulation.
+ros is a container based on the ROS Melodic docker image. This is provided for convenience, but not used for running the final simulation.
 
 #### gazebo <!-- omit in toc -->
 
-Similarly to ros, gazebo is the production container for gazebo. This is meant to run our final simulation.
+gazebo is a container based on the Gazebo Server 9 docker image. This is provided for convenience, but not used for running the final simulation.
+
+#### gear <!-- omit in toc -->
+
+The GEAR container includes the Gazebo Environment for Agile Robotics from the Agile Robotics for Industrial Automation Competition 2019. This is used for running the final simulation.
 
 ### GUI Support
 
-If using VcXsrv for Windows to enable GUI applications, run XLaunch using [config.xlaunch](utils/config.xlaunch). This will enable the following settings:
+If using VcXsrv for Windows to enable GUI applications, run XLaunch using [config.xlaunch](https://github.com/EECS-467-W20-RRRobot-Project/RRRobot/blob/master/utils/config.xlaunch). This will enable the following settings:
 
 - Display Settings
   - Multiple Windows
@@ -83,7 +102,12 @@ libGL error: failed to load driver: swrast
 
 As mentioned in [No libGL libraries when running Gazebo from ROS](https://github.com/microsoft/WSL/issues/3644#issuecomment-434556680), this can be resolved by disabling `No native OpenGL` in the VcXsrv configuration.
 
-Additionally, be sure to update the `IP_ADDRESS` variable in [.env](src/.env) with your computer's IP Address to enable X forwarding.
+You will need to update your environment variable with your IP Address to enable X forwarding. You can create a file called **.env** in the same directory as the docker-compose.yml file with your IP address as shown below. You can also edit the [docker-compose.yml](https://github.com/EECS-467-W20-RRRobot-Project/RRRobot/blob/master/docker_env/docker-compose.yml) to remove the dependence on the `IP_ADDRESS` environment variable.
+
+```
+# .env for docker-compose
+IP_ADDRESS=<IP Address Here>
+```
 
 ### Running Docker Containers
 
@@ -95,5 +119,35 @@ Additionally, be sure to update the `IP_ADDRESS` variable in [.env](src/.env) wi
     - Other: `docker-machine env`
 3. Navigate to src folder
     - `cd /PATH/TO/rrrobot/src`
-4. Use Docker Compose to run a service (refer to [docker-compose.yml](src/docker-compose.yml) or [Docker Compose Services](#docker-compose-services))
+4. Use Docker Compose to run a service (refer to [docker-compose.yml](https://github.com/EECS-467-W20-RRRobot-Project/RRRobot/blob/master/docker_env/docker-compose.yml) or [Docker Compose Services](#docker-compose-services))
    - `docker-compose run --rm <service_name>`
+
+### Gazebo Grasping Simulation
+
+1. Source ROS Setup
+   - `source /opt/ros/melodic/setup.bash`
+2. Build the drivers for the simulation
+   - `cd /app/rrrobot_src/src/simulation_drivers/`
+   - `source build.sh`
+3. Start ros master node
+   - `roscore &`
+4. Run the gazebo simulator - this will bring up gazebo with a robotic arm
+   - `gazebo /app/rrrobot_src/world/rrrobot.world`
+5. Run control and perception programs
+
+### Tips & Tricks
+
+- Clear space on docker machine
+  - `docker system prune --volumes`
+- See running containers
+  - `docker ps`
+- Attach a new terminal to a running container
+  - `docker exec -it -u root <container> bash`
+
+## Source Code Documentation
+
+Documentation on our source code can be found on the [Documentation](documentation.md) page.
+
+## GEAR Final Simulation
+
+Instructions for running our final GEAR simulation can be found on the [GEAR](gear.md) page.

run_rrrobot_image.sh

@@ -0,0 +1,3 @@
+xhost +local:docker #rrrobot-env
+docker run -it --rm --device=/dev/input/event4 --device=/dev/dri:/dev/dri -e DISPLAY=$DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix -v $PWD/rrrobot_src:/home/rrrobot/rrrobot_src -h rrrobot-env eecs467:rrrobot
+xhost -local:docker #rrrobot-env

src/.env

@@ -1,3 +0,0 @@
-# .env for docker-compose
-
-IP_ADDRESS=192.168.1.14

src/gazebo/Dockerfile

@@ -1,6 +0,0 @@
-# Gazebo Dockerfile
-
-# Use official image for Gazebo 9.x 
-FROM gazebo:gzserver9
-
-CMD ["/bin/bash"]

src/rrrobot_ws/build.sh

@@ -0,0 +1,8 @@
+#!/bin/bash
+
+catkin_make clean &&
+catkin_make &&
+catkin_make install &&
+
+source devel/setup.bash
+export GAZEBO_RESOURCE_PATH=$(pwd)/world:$GAZEBO_RESOURCE_PATH

src/rrrobot_ws/run_all.sh

@@ -0,0 +1,60 @@
+#!/bin/bash
+
+# Build Workspace
+cd /app/rrrobot_ws
+catkin_make clean
+catkin_make
+source devel/setup.bash
+export GAZEBO_RESOURCE_PATH=$(pwd)/world:$GAZEBO_RESOURCE_PATH
+
+# ARIAC Environment
+cd /app/rrrobot_ws/src/rrrobot/scripts
+./rrrobot_run_no_build.sh &
+sleep 25
+rosparam set /use_sim_time true
+
+# CV Model
+cd /app/rrrobot_ws/src/rrrobot/src
+python3 cv_model.py &
+sleep 10
+
+# Arm Controller Node
+cd /app/rrrobot_ws
+rosrun rrrobot arm_controller_node &
+sleep 3
+
+# Depth Camera Node
+rosrun rrrobot depth_camera_node &
+sleep 3
+
+# RRRobot Node
+rosrun rrrobot rrrobot_node &
+sleep 3
+
+# Object Spawner
+rosrun rrrobot object_spawner_node &
+sleep 3
+
+# Start Competition
+source /opt/ros/melodic/setup.bash
+rosservice call /ariac/start_competition
+sleep 1
+rosservice call /ariac/conveyor/control "power: 100"
+sleep 1
+rosservice call /ariac/arm1/gripper/control "enable: false"
+sleep 1
+rosservice call /ariac/arm1/gripper/control "enable: true"
+sleep 1
+rosservice call /ariac/arm1/gripper/control "enable: false"
+
+#/app/rrrobot_ws/src/rrrobot/scripts/rrrobot_run_no_build.sh &
+# cd /app/rrrobot_ws/src/rrrobot/src
+# python3 cv_model.py &
+# cd /app/rrrobot_ws/devel/lib/rrrobot
+# ./rrrobot_node &
+# ./depth_camera_node >> /dev/null &
+# ./object_spawner_node &
+# ./arm_controller_node &
+# rostopic echo /arm_controller/destination &
+# rostopic echo /desired_grasp_pose &
+# rostopic echo /cv_model &

src/rrrobot_ws/src/CMakeLists.txt

@@ -0,0 +1,67 @@
+# toplevel CMakeLists.txt for a catkin workspace
+# catkin/cmake/toplevel.cmake
+
+cmake_minimum_required(VERSION 3.0.2)
+
+set(CATKIN_TOPLEVEL TRUE)
+
+# search for catkin within the workspace
+set(_cmd "catkin_find_pkg" "catkin" "${CMAKE_SOURCE_DIR}")
+execute_process(COMMAND ${_cmd}
+  RESULT_VARIABLE _res
+  OUTPUT_VARIABLE _out
+  ERROR_VARIABLE _err
+  OUTPUT_STRIP_TRAILING_WHITESPACE
+  ERROR_STRIP_TRAILING_WHITESPACE
+)
+if(NOT _res EQUAL 0 AND NOT _res EQUAL 2)
+  # searching fot catkin resulted in an error
+  string(REPLACE ";" " " _cmd_str "${_cmd}")
+  message(FATAL_ERROR "Search for 'catkin' in workspace failed (${_cmd_str}): ${_err}")
+endif()
+
+# include catkin from workspace or via find_package()
+if(_res EQUAL 0)
+  set(catkin_EXTRAS_DIR "${CMAKE_SOURCE_DIR}/${_out}/cmake")
+  # include all.cmake without add_subdirectory to let it operate in same scope
+  include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE)
+  add_subdirectory("${_out}")
+
+else()
+  # use either CMAKE_PREFIX_PATH explicitly passed to CMake as a command line argument
+  # or CMAKE_PREFIX_PATH from the environment
+  if(NOT DEFINED CMAKE_PREFIX_PATH)
+    if(NOT "$ENV{CMAKE_PREFIX_PATH}" STREQUAL "")
+      if(NOT WIN32)
+        string(REPLACE ":" ";" CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})
+      else()
+        set(CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})
+      endif()
+    endif()
+  endif()
+
+  # list of catkin workspaces
+  set(catkin_search_path "")
+  foreach(path ${CMAKE_PREFIX_PATH})
+    if(EXISTS "${path}/.catkin")
+      list(FIND catkin_search_path ${path} _index)
+      if(_index EQUAL -1)
+        list(APPEND catkin_search_path ${path})
+      endif()
+    endif()
+  endforeach()
+
+  # search for catkin in all workspaces
+  set(CATKIN_TOPLEVEL_FIND_PACKAGE TRUE)
+  find_package(catkin QUIET
+    NO_POLICY_SCOPE
+    PATHS ${catkin_search_path}
+    NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH)
+  unset(CATKIN_TOPLEVEL_FIND_PACKAGE)
+
+  if(NOT catkin_FOUND)
+    message(FATAL_ERROR "find_package(catkin) failed. catkin was neither found in the workspace nor in the CMAKE_PREFIX_PATH. One reason may be that no ROS setup.sh was sourced before.")
+  endif()
+endif()
+
+catkin_workspace()

src/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/model.config

@@ -0,0 +1,11 @@
+<?xml version="1.0" ?>
+<model>
+    <name>fanuc_robotic_arm_with_gripper</name>
+    <version>1.0</version>
+    <sdf version="1.6">model.sdf</sdf>
+    <author>
+        <name></name>
+        <email></email>
+    </author>
+    <description></description>
+</model>

src/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/model.sdf

@@ -0,0 +1,956 @@
+<?xml version='1.0'?>
+<sdf version='1.6'>
+  <model name='fanuc_robotic_arm_with_gripper'>
+    <link name='base'>
+      <pose frame=''>0 0 0 3.14159 -0 0</pose>
+      <inertial>
+        <mass>86.082</mass>
+        <inertia>
+          <ixx>4.3404</ixx>
+          <ixy>0</ixy>
+          <ixz>0.0020207</ixz>
+          <iyy>2.4866</iyy>
+          <iyz>0</iyz>
+          <izz>6.1574</izz>
+        </inertia>
+        <pose frame=''>-0.003895 -0.062844 -0.11068 0 -0 0</pose>
+      </inertial>
+      <self_collide>1</self_collide>
+      <kinematic>0</kinematic>
+      <gravity>0</gravity>
+      <enable_wind>0</enable_wind>
+      <visual name='visual'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <mesh>
+            <uri>/home/rrrobot/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/base_link.STL</uri>
+            <scale>1 1 1</scale>
+          </mesh>
+        </geometry>
+        <material>
+          <lighting>1</lighting>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <shader type='pixel'>
+            <normal_map>__default__</normal_map>
+          </shader>
+          <ambient>0.3 0.3 0.3 1</ambient>
+          <diffuse>0.7 0.7 0.7 1</diffuse>
+          <specular>0.01 0.01 0.01 1</specular>
+          <emissive>0 0 0 1</emissive>
+        </material>
+        <transparency>0</transparency>
+        <cast_shadows>1</cast_shadows>
+      </visual>
+      <collision name='collision'>
+        <laser_retro>0</laser_retro>
+        <max_contacts>10</max_contacts>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <mesh>
+            <uri>/home/rrrobot/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/base_link.STL</uri>
+            <scale>1 1 1</scale>
+          </mesh>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>1</mu>
+              <mu2>1</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0</slip1>
+              <slip2>0</slip2>
+            </ode>
+            <torsional>
+              <coefficient>1</coefficient>
+              <patch_radius>0</patch_radius>
+              <surface_radius>0</surface_radius>
+              <use_patch_radius>1</use_patch_radius>
+              <ode>
+                <slip>0</slip>
+              </ode>
+            </torsional>
+          </friction>
+          <bounce>
+            <restitution_coefficient>0</restitution_coefficient>
+            <threshold>1e+06</threshold>
+          </bounce>
+          <contact>
+            <collide_without_contact>0</collide_without_contact>
+            <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+            <collide_bitmask>1</collide_bitmask>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0</min_depth>
+            </ode>
+            <bullet>
+              <split_impulse>1</split_impulse>
+              <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+            </bullet>
+          </contact>
+        </surface>
+      </collision>
+    </link>
+    <link name='link_1'>
+      <pose frame=''>0.001715 0.003779 0.661683 1.5708 -0 0</pose>
+      <inertial>
+        <mass>266.27</mass>
+        <inertia>
+          <ixx>279.51</ixx>
+          <ixy>0</ixy>
+          <ixz>4.6477</ixz>
+          <iyy>43.386</iyy>
+          <iyz>34.265</iyz>
+          <izz>255.26</izz>
+        </inertia>
+        <pose frame=''>0.093634 -0.51398 -0.1614 0 -0 0</pose>
+      </inertial>
+      <self_collide>1</self_collide>
+      <kinematic>0</kinematic>
+      <gravity>0</gravity>
+      <enable_wind>0</enable_wind>
+      <visual name='visual'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <mesh>
+            <uri>/home/rrrobot/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_1.STL</uri>
+            <scale>1 1 1</scale>
+          </mesh>
+        </geometry>
+        <material>
+          <lighting>1</lighting>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <shader type='pixel'>
+            <normal_map>__default__</normal_map>
+          </shader>
+          <ambient>0.3 0.3 0.3 1</ambient>
+          <diffuse>0.7 0.7 0.7 1</diffuse>
+          <specular>0.01 0.01 0.01 1</specular>
+          <emissive>0 0 0 1</emissive>
+        </material>
+        <transparency>0</transparency>
+        <cast_shadows>1</cast_shadows>
+      </visual>
+      <collision name='collision'>
+        <laser_retro>0</laser_retro>
+        <max_contacts>10</max_contacts>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <mesh>
+            <uri>/home/rrrobot/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_1.STL</uri>
+            <scale>1 1 1</scale>
+          </mesh>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>1</mu>
+              <mu2>1</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0</slip1>
+              <slip2>0</slip2>
+            </ode>
+            <torsional>
+              <coefficient>1</coefficient>
+              <patch_radius>0</patch_radius>
+              <surface_radius>0</surface_radius>
+              <use_patch_radius>1</use_patch_radius>
+              <ode>
+                <slip>0</slip>
+              </ode>
+            </torsional>
+          </friction>
+          <bounce>
+            <restitution_coefficient>0</restitution_coefficient>
+            <threshold>1e+06</threshold>
+          </bounce>
+          <contact>
+            <collide_without_contact>0</collide_without_contact>
+            <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+            <collide_bitmask>1</collide_bitmask>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0</min_depth>
+            </ode>
+            <bullet>
+              <split_impulse>1</split_impulse>
+              <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+            </bullet>
+          </contact>
+        </surface>
+      </collision>
+    </link>
+    <link name='link_2'>
+      <pose frame=''>-0.068455 0.275759 0.315754 -1.571 0 0</pose>
+      <inertial>
+        <mass>60.795</mass>
+        <inertia>
+          <ixx>8.1939</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.76659</iyy>
+          <iyz>0</iyz>
+          <izz>8.0226</izz>
+        </inertia>
+        <pose frame=''>0.25344 -1.009 -0.57114 0 -0 0</pose>
+      </inertial>
+      <self_collide>1</self_collide>
+      <kinematic>0</kinematic>
+      <gravity>0</gravity>
+      <enable_wind>0</enable_wind>
+      <visual name='visual'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <mesh>
+            <uri>/home/rrrobot/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_2.STL</uri>
+            <scale>1 1 1</scale>
+          </mesh>
+        </geometry>
+        <material>
+          <lighting>1</lighting>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <shader type='pixel'>
+            <normal_map>__default__</normal_map>
+          </shader>
+          <ambient>0.3 0.3 0.3 1</ambient>
+          <diffuse>0.7 0.7 0.7 1</diffuse>
+          <specular>0.01 0.01 0.01 1</specular>
+          <emissive>0 0 0 1</emissive>
+        </material>
+        <transparency>0</transparency>
+        <cast_shadows>1</cast_shadows>
+      </visual>
+      <collision name='collision'>
+        <laser_retro>0</laser_retro>
+        <max_contacts>10</max_contacts>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <mesh>
+            <uri>/home/rrrobot/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_2.STL</uri>
+            <scale>1 1 1</scale>
+          </mesh>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>1</mu>
+              <mu2>1</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0</slip1>
+              <slip2>0</slip2>
+            </ode>
+            <torsional>
+              <coefficient>1</coefficient>
+              <patch_radius>0</patch_radius>
+              <surface_radius>0</surface_radius>
+              <use_patch_radius>1</use_patch_radius>
+              <ode>
+                <slip>0</slip>
+              </ode>
+            </torsional>
+          </friction>
+          <bounce>
+            <restitution_coefficient>0</restitution_coefficient>
+            <threshold>1e+06</threshold>
+          </bounce>
+          <contact>
+            <collide_without_contact>0</collide_without_contact>
+            <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+            <collide_bitmask>1</collide_bitmask>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0</min_depth>
+            </ode>
+            <bullet>
+              <split_impulse>1</split_impulse>
+              <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+            </bullet>
+          </contact>
+        </surface>
+      </collision>
+    </link>
+    <link name='link_3'>
+      <pose frame=''>-0.00025 -0.94659 5.45119 1.5708 -0 0</pose>
+      <inertial>
+        <mass>63.612</mass>
+        <inertia>
+          <ixx>6.6124</ixx>
+          <ixy>0.099584</ixy>
+          <ixz>0</ixz>
+          <iyy>5.9295</iyy>
+          <iyz>0</iyz>
+          <izz>1.9517</izz>
+        </inertia>
+        <pose frame=''>0.00756 -3.4862 -0.40714 0 -0 0</pose>
+      </inertial>
+      <self_collide>1</self_collide>
+      <kinematic>0</kinematic>
+      <gravity>0</gravity>
+      <enable_wind>0</enable_wind>
+      <visual name='visual'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <mesh>
+            <uri>/home/rrrobot/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_3.STL</uri>
+            <scale>1 1 1</scale>
+          </mesh>
+        </geometry>
+        <material>
+          <lighting>1</lighting>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <shader type='pixel'>
+            <normal_map>__default__</normal_map>
+          </shader>
+          <ambient>0.3 0.3 0.3 1</ambient>
+          <diffuse>0.7 0.7 0.7 1</diffuse>
+          <specular>0.01 0.01 0.01 1</specular>
+          <emissive>0 0 0 1</emissive>
+        </material>
+        <transparency>0</transparency>
+        <cast_shadows>1</cast_shadows>
+      </visual>
+      <collision name='collision'>
+        <laser_retro>0</laser_retro>
+        <max_contacts>10</max_contacts>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <mesh>
+            <uri>/home/rrrobot/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_3.STL</uri>
+            <scale>1 1 1</scale>
+          </mesh>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>1</mu>
+              <mu2>1</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0</slip1>
+              <slip2>0</slip2>
+            </ode>
+            <torsional>
+              <coefficient>1</coefficient>
+              <patch_radius>0</patch_radius>
+              <surface_radius>0</surface_radius>
+              <use_patch_radius>1</use_patch_radius>
+              <ode>
+                <slip>0</slip>
+              </ode>
+            </torsional>
+          </friction>
+          <bounce>
+            <restitution_coefficient>0</restitution_coefficient>
+            <threshold>1e+06</threshold>
+          </bounce>
+          <contact>
+            <collide_without_contact>0</collide_without_contact>
+            <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+            <collide_bitmask>1</collide_bitmask>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0</min_depth>
+            </ode>
+            <bullet>
+              <split_impulse>1</split_impulse>
+              <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+            </bullet>
+          </contact>
+        </surface>
+      </collision>
+    </link>
+    <link name='link_4'>
+      <pose frame=''>-0.000257 -3.97477 2.07064 -1.5708 7e-06 3.14159</pose>
+      <inertial>
+        <mass>6.6264</mass>
+        <inertia>
+          <ixx>0.089718</ixx>
+          <ixy>0</ixy>
+          <ixz>0.017666</ixz>
+          <iyy>0.092881</iyy>
+          <iyz>3e-08</iyz>
+          <izz>0.028564</izz>
+        </inertia>
+        <pose frame=''>-0.028814 0 -2.454 0 -0 0</pose>
+      </inertial>
+      <self_collide>1</self_collide>
+      <kinematic>0</kinematic>
+      <gravity>0</gravity>
+      <enable_wind>0</enable_wind>
+      <visual name='visual'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <mesh>
+            <uri>/home/rrrobot/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_4.STL</uri>
+            <scale>1 1 1</scale>
+          </mesh>
+        </geometry>
+        <material>
+          <lighting>1</lighting>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <shader type='pixel'>
+            <normal_map>__default__</normal_map>
+          </shader>
+          <ambient>0.3 0.3 0.3 1</ambient>
+          <diffuse>0.7 0.7 0.7 1</diffuse>
+          <specular>0.01 0.01 0.01 1</specular>
+          <emissive>0 0 0 1</emissive>
+        </material>
+        <transparency>0</transparency>
+        <cast_shadows>1</cast_shadows>
+      </visual>
+      <collision name='collision'>
+        <laser_retro>0</laser_retro>
+        <max_contacts>10</max_contacts>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <mesh>
+            <uri>/home/rrrobot/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_4.STL</uri>
+            <scale>1 1 1</scale>
+          </mesh>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>1</mu>
+              <mu2>1</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0</slip1>
+              <slip2>0</slip2>
+            </ode>
+            <torsional>
+              <coefficient>1</coefficient>
+              <patch_radius>0</patch_radius>
+              <surface_radius>0</surface_radius>
+              <use_patch_radius>1</use_patch_radius>
+              <ode>
+                <slip>0</slip>
+              </ode>
+            </torsional>
+          </friction>
+          <bounce>
+            <restitution_coefficient>0</restitution_coefficient>
+            <threshold>1e+06</threshold>
+          </bounce>
+          <contact>
+            <collide_without_contact>0</collide_without_contact>
+            <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+            <collide_bitmask>1</collide_bitmask>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0</min_depth>
+            </ode>
+            <bullet>
+              <split_impulse>1</split_impulse>
+              <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+            </bullet>
+          </contact>
+        </surface>
+      </collision>
+    </link>
+    <link name='link_5'>
+      <pose frame=''>0.000743 -4.767 2.06498 -1.5708 7e-06 3.14159</pose>
+      <inertial>
+        <mass>5.1229</mass>
+        <inertia>
+          <ixx>0.038195</ixx>
+          <ixy>4e-07</ixy>
+          <ixz>0</ixz>
+          <iyy>0.066675</iyy>
+          <iyz>6e-07</iyz>
+          <izz>0.04821</izz>
+        </inertia>
+        <pose frame=''>-0.010371 1e-06 -3.0669 0 -0 0</pose>
+      </inertial>
+      <self_collide>1</self_collide>
+      <kinematic>0</kinematic>
+      <gravity>0</gravity>
+      <enable_wind>0</enable_wind>
+      <visual name='visual'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <mesh>
+            <uri>/home/rrrobot/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_5.STL</uri>
+            <scale>1 1 1</scale>
+          </mesh>
+        </geometry>
+        <material>
+          <lighting>1</lighting>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <shader type='pixel'>
+            <normal_map>__default__</normal_map>
+          </shader>
+          <ambient>0.3 0.3 0.3 1</ambient>
+          <diffuse>0.7 0.7 0.7 1</diffuse>
+          <specular>0.01 0.01 0.01 1</specular>
+          <emissive>0 0 0 1</emissive>
+        </material>
+        <transparency>0</transparency>
+        <cast_shadows>1</cast_shadows>
+      </visual>
+      <collision name='collision'>
+        <laser_retro>0</laser_retro>
+        <max_contacts>10</max_contacts>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <mesh>
+            <uri>/home/rrrobot/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_5.STL</uri>
+            <scale>1 1 1</scale>
+          </mesh>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>1</mu>
+              <mu2>1</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0</slip1>
+              <slip2>0</slip2>
+            </ode>
+            <torsional>
+              <coefficient>1</coefficient>
+              <patch_radius>0</patch_radius>
+              <surface_radius>0</surface_radius>
+              <use_patch_radius>1</use_patch_radius>
+              <ode>
+                <slip>0</slip>
+              </ode>
+            </torsional>
+          </friction>
+          <bounce>
+            <restitution_coefficient>0</restitution_coefficient>
+            <threshold>1e+06</threshold>
+          </bounce>
+          <contact>
+            <collide_without_contact>0</collide_without_contact>
+            <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+            <collide_bitmask>1</collide_bitmask>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0</min_depth>
+            </ode>
+            <bullet>
+              <split_impulse>1</split_impulse>
+              <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+            </bullet>
+          </contact>
+        </surface>
+      </collision>
+    </link>
+    <link name='link_6'>
+      <pose frame=''>-2.06426 -5.37015 4.13075 1.88334 1.57078 0.312546</pose>
+      <inertial>
+        <mass>0.72893</mass>
+        <inertia>
+          <ixx>0.00131</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.0012978</iyy>
+          <iyz>0</iyz>
+          <izz>0.0024341</izz>
+        </inertia>
+        <pose frame=''>2.0657 2.0642 -3.5441 0 -0 0</pose>
+      </inertial>
+      <self_collide>1</self_collide>
+      <kinematic>0</kinematic>
+      <gravity>0</gravity>
+      <enable_wind>0</enable_wind>
+      <visual name='visual'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <mesh>
+            <uri>/home/rrrobot/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_6.STL</uri>
+            <scale>1 1 1</scale>
+          </mesh>
+        </geometry>
+        <material>
+          <lighting>1</lighting>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <shader type='pixel'>
+            <normal_map>__default__</normal_map>
+          </shader>
+          <ambient>0.3 0.3 0.3 1</ambient>
+          <diffuse>0.7 0.7 0.7 1</diffuse>
+          <specular>0.01 0.01 0.01 1</specular>
+          <emissive>0 0 0 1</emissive>
+        </material>
+        <transparency>0</transparency>
+        <cast_shadows>1</cast_shadows>
+      </visual>
+      <collision name='collision'>
+        <laser_retro>0</laser_retro>
+        <max_contacts>10</max_contacts>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <mesh>
+            <uri>/home/rrrobot/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_6.STL</uri>
+            <scale>1 1 1</scale>
+          </mesh>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>1</mu>
+              <mu2>1</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0</slip1>
+              <slip2>0</slip2>
+            </ode>
+            <torsional>
+              <coefficient>1</coefficient>
+              <patch_radius>0</patch_radius>
+              <surface_radius>0</surface_radius>
+              <use_patch_radius>1</use_patch_radius>
+              <ode>
+                <slip>0</slip>
+              </ode>
+            </torsional>
+          </friction>
+          <bounce>
+            <restitution_coefficient>0</restitution_coefficient>
+            <threshold>1e+06</threshold>
+          </bounce>
+          <contact>
+            <collide_without_contact>0</collide_without_contact>
+            <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+            <collide_bitmask>1</collide_bitmask>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0</min_depth>
+            </ode>
+            <bullet>
+              <split_impulse>1</split_impulse>
+              <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+            </bullet>
+          </contact>
+        </surface>
+      </collision>
+    </link>
+    <link name='link_7'>
+      <inertial>
+        <mass>0.72893</mass>
+        <inertia>
+          <ixx>0.00131</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.0012978</iyy>
+          <iyz>0</iyz>
+          <izz>0.0024341</izz>
+        </inertia>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+      </inertial>
+      <pose frame=''>-0.000802 -1.86676 2.06269 -1e-06 -1e-06 0</pose>
+      <gravity>0</gravity>
+      <self_collide>0</self_collide>
+      <kinematic>0</kinematic>
+      <enable_wind>0</enable_wind>
+    </link>
+    <joint name='elbow_joint' type='revolute'>
+      <parent>link_2</parent>
+      <child>link_3</child>
+      <pose frame=''>0 -3.65 -0.6 0 -0 0</pose>
+      <axis>
+        <xyz>1 0 0</xyz>
+        <use_parent_model_frame>0</use_parent_model_frame>
+        <limit>
+          <lower>-1.79769e+308</lower>
+          <upper>1.79769e+308</upper>
+          <effort>-2e+07</effort>
+          <velocity>-1</velocity>
+        </limit>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>0</spring_stiffness>
+          <damping>0</damping>
+          <friction>0</friction>
+        </dynamics>
+      </axis>
+      <physics>
+        <ode>
+          <limit>
+            <cfm>0</cfm>
+            <erp>0.2</erp>
+          </limit>
+          <suspension>
+            <cfm>0</cfm>
+            <erp>0.2</erp>
+          </suspension>
+        </ode>
+      </physics>
+    </joint>
+    <joint name='end_effector_pivot' type='revolute'>
+      <parent>link_5</parent>
+      <child>link_6</child>
+      <pose frame=''>2.065 2.065 -3.5 0 -0 0</pose>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <use_parent_model_frame>0</use_parent_model_frame>
+        <limit>
+          <lower>-1.79769e+308</lower>
+          <upper>1.79769e+308</upper>
+          <effort>-2e+07</effort>
+          <velocity>-1</velocity>
+        </limit>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>0</spring_stiffness>
+          <damping>0</damping>
+          <friction>0</friction>
+        </dynamics>
+      </axis>
+      <physics>
+        <ode>
+          <limit>
+            <cfm>0</cfm>
+            <erp>0.2</erp>
+          </limit>
+          <suspension>
+            <cfm>0</cfm>
+            <erp>0.2</erp>
+          </suspension>
+        </ode>
+      </physics>
+    </joint>
+    <joint name='link_6_JOINT_0' type='fixed'>
+      <parent>link_6</parent>
+      <child>link_7</child>
+      <pose frame=''>0 0 0 0 -0 0</pose>
+      <physics>
+        <ode>
+          <limit>
+            <cfm>0</cfm>
+            <erp>0.2</erp>
+          </limit>
+          <suspension>
+            <cfm>0</cfm>
+            <erp>0.2</erp>
+          </suspension>
+        </ode>
+      </physics>
+    </joint>
+    <joint name='shoulder_joint' type='revolute'>
+      <parent>link_1</parent>
+      <child>link_2</child>
+      <pose frame=''>0 -0.41 -0.61 0 -0 0</pose>
+      <axis>
+        <xyz>1 0 0</xyz>
+        <use_parent_model_frame>0</use_parent_model_frame>
+        <limit>
+          <lower>-1.79769e+308</lower>
+          <upper>1.79769e+308</upper>
+          <effort>-2e+07</effort>
+          <velocity>-1</velocity>
+        </limit>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>0</spring_stiffness>
+          <damping>0</damping>
+          <friction>0</friction>
+        </dynamics>
+      </axis>
+      <physics>
+        <ode>
+          <limit>
+            <cfm>0</cfm>
+            <erp>0.2</erp>
+          </limit>
+          <suspension>
+            <cfm>0</cfm>
+            <erp>0.2</erp>
+          </suspension>
+        </ode>
+      </physics>
+    </joint>
+    <joint name='shoulder_pivot' type='revolute'>
+      <parent>base</parent>
+      <child>link_1</child>
+      <pose frame=''>0 0 0 0 -0 0</pose>
+      <axis>
+        <xyz>0 1 0</xyz>
+        <use_parent_model_frame>0</use_parent_model_frame>
+        <limit>
+          <lower>-1.79769e+308</lower>
+          <upper>1.79769e+308</upper>
+          <effort>-2e+07</effort>
+          <velocity>-1</velocity>
+        </limit>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>0</spring_stiffness>
+          <damping>0</damping>
+          <friction>0</friction>
+        </dynamics>
+      </axis>
+      <physics>
+        <ode>
+          <limit>
+            <cfm>0</cfm>
+            <erp>0.2</erp>
+          </limit>
+          <suspension>
+            <cfm>0</cfm>
+            <erp>0.2</erp>
+          </suspension>
+        </ode>
+      </physics>
+    </joint>
+    <joint name='wrist_joint' type='revolute'>
+      <parent>link_4</parent>
+      <child>link_5</child>
+      <pose frame=''>0 0 -3.15 0 -0 0</pose>
+      <axis>
+        <xyz>1 0 0</xyz>
+        <use_parent_model_frame>0</use_parent_model_frame>
+        <limit>
+          <lower>-1.79769e+308</lower>
+          <upper>1.79769e+308</upper>
+          <effort>-2e+07</effort>
+          <velocity>-1</velocity>
+        </limit>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>0</spring_stiffness>
+          <damping>0</damping>
+          <friction>0</friction>
+        </dynamics>
+      </axis>
+      <physics>
+        <ode>
+          <limit>
+            <cfm>0</cfm>
+            <erp>0.2</erp>
+          </limit>
+          <suspension>
+            <cfm>0</cfm>
+            <erp>0.2</erp>
+          </suspension>
+        </ode>
+      </physics>
+    </joint>
+    <joint name='wrist_pivot' type='revolute'>
+      <parent>link_3</parent>
+      <child>link_4</child>
+      <pose frame=''>0 0 -2.7 0 -0 0</pose>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <use_parent_model_frame>0</use_parent_model_frame>
+        <limit>
+          <lower>-1.79769e+308</lower>
+          <upper>1.79769e+308</upper>
+          <effort>-2e+07</effort>
+          <velocity>-1</velocity>
+        </limit>
+        <dynamics>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>0</spring_stiffness>
+          <damping>0</damping>
+          <friction>0</friction>
+        </dynamics>
+      </axis>
+      <physics>
+        <ode>
+          <limit>
+            <cfm>0</cfm>
+            <erp>0.2</erp>
+          </limit>
+          <suspension>
+            <cfm>0</cfm>
+            <erp>0.2</erp>
+          </suspension>
+        </ode>
+      </physics>
+    </joint>
+    <joint name='gripper_joint' type='fixed'>
+      <parent>link_6</parent>
+      <child>link_7</child>
+    </joint>
+    <joint name='world_fix' type='fixed'>
+      <parent>world</parent>
+      <child>base</child>
+    </joint>
+    <static>0</static>
+    <allow_auto_disable>0</allow_auto_disable>
+    <plugin name='arm_control' filename='libarm_motors.so'/>
+    <plugin name='gazebo_ros_vacuum_gripper' filename='libgazebo_ros_vacuum_gripper.so'>
+      <robotNamespace>/arm_node/vacuum_gripper</robotNamespace>
+      <bodyName>link_7</bodyName>
+      <topicName>grasping</topicName>
+    </plugin>
+    <plugin name='joint_angles' filename='libarm_angles.so'/>
+  </model>
+</sdf>

src/rrrobot_ws/src/gazebo_models/generic_recyclable/model.config

@@ -0,0 +1,11 @@
+<?xml version="1.0" ?>
+<model>
+    <name>generic_recyclable</name>
+    <version>1.0</version>
+    <sdf version="1.6">model.sdf</sdf>
+    <author>
+        <name></name>
+        <email></email>
+    </author>
+    <description></description>
+</model>

src/rrrobot_ws/src/gazebo_models/generic_recyclable/model.sdf

@@ -0,0 +1,104 @@
+<?xml version='1.0'?>
+<sdf version='1.6'>
+  <model name='generic_recyclable'>
+    <link name='link'>
+      <inertial>
+        <mass>0.208095</mass>
+        <inertia>
+          <ixx>0.0180922</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.0180922</iyy>
+          <iyz>0</iyz>
+          <izz>0.0346825</izz>
+        </inertia>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+      </inertial>
+      <self_collide>0</self_collide>
+      <enable_wind>0</enable_wind>
+      <kinematic>0</kinematic>
+      <pose frame=''>0 0 0 0 -0 0</pose>
+      <gravity>1</gravity>
+      <visual name='visual'>
+        <geometry>
+          <box>
+            <size>0.05 0.05 0.05</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <name>Gazebo/Grey</name>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+          </script>
+          <shader type='pixel'>
+            <normal_map>__default__</normal_map>
+          </shader>
+          <ambient>0 1 0 1</ambient>
+          <diffuse>0.7 0.7 0.7 1</diffuse>
+          <specular>0.01 0.01 0.01 1</specular>
+          <emissive>0 0 0 1</emissive>
+        </material>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <transparency>0</transparency>
+        <cast_shadows>1</cast_shadows>
+      </visual>
+      <collision name='collision'>
+        <laser_retro>0</laser_retro>
+        <max_contacts>10</max_contacts>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.05 0.05 0.05</size>
+          </box>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>1</mu>
+              <mu2>1</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0</slip1>
+              <slip2>0</slip2>
+            </ode>
+            <torsional>
+              <coefficient>1</coefficient>
+              <patch_radius>0</patch_radius>
+              <surface_radius>0</surface_radius>
+              <use_patch_radius>1</use_patch_radius>
+              <ode>
+                <slip>0</slip>
+              </ode>
+            </torsional>
+          </friction>
+          <bounce>
+            <restitution_coefficient>0</restitution_coefficient>
+            <threshold>1e+06</threshold>
+          </bounce>
+          <contact>
+            <collide_without_contact>0</collide_without_contact>
+            <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+            <collide_bitmask>1</collide_bitmask>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0</min_depth>
+            </ode>
+            <bullet>
+              <split_impulse>1</split_impulse>
+              <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+            </bullet>
+          </contact>
+        </surface>
+      </collision>
+    </link>
+    <static>0</static>
+    <allow_auto_disable>1</allow_auto_disable>
+  </model>
+</sdf>

src/rrrobot_ws/src/gazebo_models/generic_trash/model.config

@@ -0,0 +1,11 @@
+<?xml version="1.0" ?>
+<model>
+    <name>generic_trash</name>
+    <version>1.0</version>
+    <sdf version="1.6">model.sdf</sdf>
+    <author>
+        <name></name>
+        <email></email>
+    </author>
+    <description></description>
+</model>

src/rrrobot_ws/src/gazebo_models/generic_trash/model.sdf

@@ -0,0 +1,104 @@
+<?xml version='1.0'?>
+<sdf version='1.6'>
+  <model name='generic_trash'>
+    <link name='link'>
+      <inertial>
+        <mass>0.208095</mass>
+        <inertia>
+          <ixx>0.0180922</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.0180922</iyy>
+          <iyz>0</iyz>
+          <izz>0.0346825</izz>
+        </inertia>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+      </inertial>
+      <self_collide>0</self_collide>
+      <enable_wind>0</enable_wind>
+      <kinematic>0</kinematic>
+      <pose frame=''>0 0 0 0 -0 0</pose>
+      <gravity>1</gravity>
+      <visual name='visual'>
+        <geometry>
+          <box>
+            <size>0.05 0.05 0.05</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <name>Gazebo/Grey</name>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+          </script>
+          <shader type='pixel'>
+            <normal_map>__default__</normal_map>
+          </shader>
+          <ambient>1 0 0 1</ambient>
+          <diffuse>0.7 0.7 0.7 1</diffuse>
+          <specular>0.01 0.01 0.01 1</specular>
+          <emissive>0 0 0 1</emissive>
+        </material>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <transparency>0</transparency>
+        <cast_shadows>1</cast_shadows>
+      </visual>
+      <collision name='collision'>
+        <laser_retro>0</laser_retro>
+        <max_contacts>10</max_contacts>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.05 0.05 0.05</size>
+          </box>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>1</mu>
+              <mu2>1</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0</slip1>
+              <slip2>0</slip2>
+            </ode>
+            <torsional>
+              <coefficient>1</coefficient>
+              <patch_radius>0</patch_radius>
+              <surface_radius>0</surface_radius>
+              <use_patch_radius>1</use_patch_radius>
+              <ode>
+                <slip>0</slip>
+              </ode>
+            </torsional>
+          </friction>
+          <bounce>
+            <restitution_coefficient>0</restitution_coefficient>
+            <threshold>1e+06</threshold>
+          </bounce>
+          <contact>
+            <collide_without_contact>0</collide_without_contact>
+            <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+            <collide_bitmask>1</collide_bitmask>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0</min_depth>
+            </ode>
+            <bullet>
+              <split_impulse>1</split_impulse>
+              <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+            </bullet>
+          </contact>
+        </surface>
+      </collision>
+    </link>
+    <static>0</static>
+    <allow_auto_disable>1</allow_auto_disable>
+  </model>
+</sdf>

src/rrrobot_ws/src/gazebo_models/model_mappings.txt

@@ -0,0 +1,10 @@
+generic_recyclable,/app/rrrobot_ws/src/gazebo_models/recycling_images/plastic.jpg,plastic
+generic_recyclable,/app/rrrobot_ws/src/gazebo_models/recycling_images/paper.jpg,paper
+generic_recyclable,/app/rrrobot_ws/src/gazebo_models/recycling_images/metal.jpg,metal
+generic_recyclable,/app/rrrobot_ws/src/gazebo_models/recycling_images/glass.jpg,glass
+generic_recyclable,/app/rrrobot_ws/src/gazebo_models/recycling_images/cardboard.jpg,cardboard
+generic_trash,/app/rrrobot_ws/src/gazebo_models/trash_images/trash_1.jpg,trash
+generic_trash,/app/rrrobot_ws/src/gazebo_models/trash_images/trash_2.jpg,trash
+generic_trash,/app/rrrobot_ws/src/gazebo_models/trash_images/trash_3.jpg,trash
+generic_trash,/app/rrrobot_ws/src/gazebo_models/trash_images/trash_4.jpg,trash
+generic_trash,/app/rrrobot_ws/src/gazebo_models/trash_images/trash_5.jpg,trash

src/rrrobot_ws/src/gazebo_models/paper_bag/model.config

@@ -0,0 +1,11 @@
+<?xml version="1.0" ?>
+<model>
+    <name>paper_bag</name>
+    <version>1.0</version>
+    <sdf version="1.6">model.sdf</sdf>
+    <author>
+        <name></name>
+        <email></email>
+    </author>
+    <description></description>
+</model>

src/rrrobot_ws/src/gazebo_models/paper_bag/model.sdf

@@ -0,0 +1,107 @@
+<?xml version='1.0'?>
+<sdf version='1.6'>
+  <model name='paper_bag'>
+    <link name='link_0'>
+      <inertial>
+        <mass>1</mass>
+        <inertia>
+          <ixx>0.166667</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.166667</iyy>
+          <iyz>0</iyz>
+          <izz>0.166667</izz>
+        </inertia>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+      </inertial>
+      <pose frame=''>0 0 0 0 -0 0</pose>
+      <gravity>1</gravity>
+      <self_collide>0</self_collide>
+      <kinematic>0</kinematic>
+      <enable_wind>0</enable_wind>
+      <visual name='visual'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <mesh>
+            <uri>/app/rrrobot_ws/src/gazebo_models/paper_bag/Paper Bag.DAE</uri>
+            <scale>1 1 1</scale>
+          </mesh>
+        </geometry>
+        <material>
+          <lighting>1</lighting>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <shader type='pixel'>
+            <normal_map>__default__</normal_map>
+          </shader>
+          <ambient>0.804 0.521 0.247 1</ambient>
+          <diffuse>0.7 0.7 0.7 1</diffuse>
+          <specular>0.01 0.01 0.01 1</specular>
+          <emissive>0 0 0 1</emissive>
+        </material>
+        <transparency>0</transparency>
+        <cast_shadows>1</cast_shadows>
+      </visual>
+      <collision name='collision'>
+        <laser_retro>0</laser_retro>
+        <max_contacts>10</max_contacts>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <mesh>
+            <uri>/app/rrrobot_ws/src/gazebo_models/paper_bag/Paper Bag.DAE</uri>
+            <scale>1 1 1</scale>
+          </mesh>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>1</mu>
+              <mu2>1</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0</slip1>
+              <slip2>0</slip2>
+            </ode>
+            <torsional>
+              <coefficient>1</coefficient>
+              <patch_radius>0</patch_radius>
+              <surface_radius>0</surface_radius>
+              <use_patch_radius>1</use_patch_radius>
+              <ode>
+                <slip>0</slip>
+              </ode>
+            </torsional>
+          </friction>
+          <bounce>
+            <restitution_coefficient>0</restitution_coefficient>
+            <threshold>1e+06</threshold>
+          </bounce>
+          <contact>
+            <collide_without_contact>0</collide_without_contact>
+            <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+            <collide_bitmask>1</collide_bitmask>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0</min_depth>
+            </ode>
+            <bullet>
+              <split_impulse>1</split_impulse>
+              <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+            </bullet>
+          </contact>
+        </surface>
+      </collision>
+    </link>
+    <static>0</static>
+    <allow_auto_disable>1</allow_auto_disable>
+  </model>
+</sdf>

src/rrrobot_ws/src/gazebo_models/plastic_bottle/model.config

@@ -0,0 +1,11 @@
+<?xml version="1.0" ?>
+<model>
+    <name>plastic_bottle</name>
+    <version>1.0</version>
+    <sdf version="1.6">model.sdf</sdf>
+    <author>
+        <name></name>
+        <email></email>
+    </author>
+    <description></description>
+</model>

src/rrrobot_ws/src/gazebo_models/plastic_bottle/model.sdf

@@ -0,0 +1,107 @@
+<?xml version='1.0'?>
+<sdf version='1.6'>
+  <model name='plastic_bottle'>
+    <link name='link_0'>
+      <inertial>
+        <mass>1</mass>
+        <inertia>
+          <ixx>0.166667</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.166667</iyy>
+          <iyz>0</iyz>
+          <izz>0.166667</izz>
+        </inertia>
+        <pose frame=''>0.172 0.63 0 0 -0 0</pose>
+      </inertial>
+      <pose frame=''>0 0 -1.05917 1.5708 -0 0</pose>
+      <gravity>1</gravity>
+      <self_collide>0</self_collide>
+      <kinematic>0</kinematic>
+      <enable_wind>0</enable_wind>
+      <visual name='visual'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <mesh>
+            <uri>/app/rrrobot_ws/src/gazebo_models/plastic_bottle/meshes/plastic_bottle.STL</uri>
+            <scale>1 1 1</scale>
+          </mesh>
+        </geometry>
+        <material>
+          <lighting>1</lighting>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <shader type='pixel'>
+            <normal_map>__default__</normal_map>
+          </shader>
+          <ambient>0.3 0.3 0.3 1</ambient>
+          <diffuse>0.7 0.7 0.7 1</diffuse>
+          <specular>0.01 0.01 0.01 1</specular>
+          <emissive>0 0 0 1</emissive>
+        </material>
+        <transparency>0</transparency>
+        <cast_shadows>1</cast_shadows>
+      </visual>
+      <collision name='collision'>
+        <laser_retro>0</laser_retro>
+        <max_contacts>10</max_contacts>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <mesh>
+            <uri>/app/rrrobot_ws/src/gazebo_models/plastic_bottle/meshes/plastic_bottle.STL</uri>
+            <scale>1 1 1</scale>
+          </mesh>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>1</mu>
+              <mu2>1</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0</slip1>
+              <slip2>0</slip2>
+            </ode>
+            <torsional>
+              <coefficient>1</coefficient>
+              <patch_radius>0</patch_radius>
+              <surface_radius>0</surface_radius>
+              <use_patch_radius>1</use_patch_radius>
+              <ode>
+                <slip>0</slip>
+              </ode>
+            </torsional>
+          </friction>
+          <bounce>
+            <restitution_coefficient>0</restitution_coefficient>
+            <threshold>1e+06</threshold>
+          </bounce>
+          <contact>
+            <collide_without_contact>0</collide_without_contact>
+            <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+            <collide_bitmask>1</collide_bitmask>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0</min_depth>
+            </ode>
+            <bullet>
+              <split_impulse>1</split_impulse>
+              <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+            </bullet>
+          </contact>
+        </surface>
+      </collision>
+    </link>
+    <static>0</static>
+    <allow_auto_disable>1</allow_auto_disable>
+  </model>
+</sdf>

src/rrrobot_ws/src/gazebo_models/pop_can/model.config

@@ -0,0 +1,11 @@
+<?xml version="1.0" ?>
+<model>
+    <name>pop_can</name>
+    <version>1.0</version>
+    <sdf version="1.6">model.sdf</sdf>
+    <author>
+        <name></name>
+        <email></email>
+    </author>
+    <description></description>
+</model>

src/rrrobot_ws/src/gazebo_models/pop_can/model.sdf

@@ -0,0 +1,107 @@
+<?xml version='1.0'?>
+<sdf version='1.6'>
+  <model name='pop_can'>
+    <link name='link_6'>
+      <inertial>
+        <mass>1</mass>
+        <inertia>
+          <ixx>0.166667</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.166667</iyy>
+          <iyz>0</iyz>
+          <izz>0.166667</izz>
+        </inertia>
+        <pose frame=''>0.02 -0.05 0.023 0 -0 0</pose>
+      </inertial>
+      <pose frame=''>-0 0 0.08 1.5708 -0 0</pose>
+      <gravity>1</gravity>
+      <self_collide>0</self_collide>
+      <kinematic>0</kinematic>
+      <enable_wind>0</enable_wind>
+      <visual name='visual'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <mesh>
+            <uri>/app/rrrobot_ws/src/gazebo_models/pop_can/original_5.stl</uri>
+            <scale>1 1 1</scale>
+          </mesh>
+        </geometry>
+        <material>
+          <lighting>1</lighting>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <shader type='pixel'>
+            <normal_map>__default__</normal_map>
+          </shader>
+          <ambient>0.3 0.3 0.3 1</ambient>
+          <diffuse>0.7 0.7 0.7 1</diffuse>
+          <specular>0.01 0.01 0.01 1</specular>
+          <emissive>0 0 0 1</emissive>
+        </material>
+        <transparency>0</transparency>
+        <cast_shadows>1</cast_shadows>
+      </visual>
+      <collision name='collision'>
+        <laser_retro>0</laser_retro>
+        <max_contacts>10</max_contacts>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <mesh>
+            <uri>/app/rrrobot_ws/src/gazebo_models/pop_can/original_5.stl</uri>
+            <scale>1 1 1</scale>
+          </mesh>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>1</mu>
+              <mu2>1</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0</slip1>
+              <slip2>0</slip2>
+            </ode>
+            <torsional>
+              <coefficient>1</coefficient>
+              <patch_radius>0</patch_radius>
+              <surface_radius>0</surface_radius>
+              <use_patch_radius>1</use_patch_radius>
+              <ode>
+                <slip>0</slip>
+              </ode>
+            </torsional>
+          </friction>
+          <bounce>
+            <restitution_coefficient>0</restitution_coefficient>
+            <threshold>1e+06</threshold>
+          </bounce>
+          <contact>
+            <collide_without_contact>0</collide_without_contact>
+            <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+            <collide_bitmask>1</collide_bitmask>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0</min_depth>
+            </ode>
+            <bullet>
+              <split_impulse>1</split_impulse>
+              <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+            </bullet>
+          </contact>
+        </surface>
+      </collision>
+    </link>
+    <static>0</static>
+    <allow_auto_disable>1</allow_auto_disable>
+  </model>
+</sdf>

src/rrrobot_ws/src/gazebo_models/pringles/model.config

@@ -0,0 +1,11 @@
+<?xml version="1.0" ?>
+<model>
+    <name>pringles</name>
+    <version>1.0</version>
+    <sdf version="1.6">model.sdf</sdf>
+    <author>
+        <name></name>
+        <email></email>
+    </author>
+    <description></description>
+</model>

src/rrrobot_ws/src/gazebo_models/pringles/model.sdf

@@ -0,0 +1,224 @@
+<?xml version='1.0'?>
+<sdf version='1.6'>
+  <model name='pringles'>
+    <link name='link_3'>
+      <inertial>
+        <mass>1</mass>
+        <inertia>
+          <ixx>0.145833</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.145833</iyy>
+          <iyz>0</iyz>
+          <izz>0.125</izz>
+        </inertia>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+      </inertial>
+      <pose frame=''>0 0 0 0 -0 0</pose>
+      <gravity>1</gravity>
+      <self_collide>0</self_collide>
+      <kinematic>0</kinematic>
+      <enable_wind>0</enable_wind>
+      <visual name='visual'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <cylinder>
+            <radius>0.07</radius>
+            <length>0.3</length>
+          </cylinder>
+        </geometry>
+        <material>
+          <lighting>1</lighting>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <shader type='pixel'>
+            <normal_map>__default__</normal_map>
+          </shader>
+          <ambient>1 0.3 0.3 1</ambient>
+          <diffuse>0.7 0.7 0.7 1</diffuse>
+          <specular>0.01 0.01 0.01 1</specular>
+          <emissive>0 0 0 1</emissive>
+        </material>
+        <transparency>0</transparency>
+        <cast_shadows>1</cast_shadows>
+      </visual>
+      <collision name='collision'>
+        <laser_retro>0</laser_retro>
+        <max_contacts>10</max_contacts>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <cylinder>
+            <radius>0.07</radius>
+            <length>0.3</length>
+          </cylinder>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>1</mu>
+              <mu2>1</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0</slip1>
+              <slip2>0</slip2>
+            </ode>
+            <torsional>
+              <coefficient>1</coefficient>
+              <patch_radius>0</patch_radius>
+              <surface_radius>0</surface_radius>
+              <use_patch_radius>1</use_patch_radius>
+              <ode>
+                <slip>0</slip>
+              </ode>
+            </torsional>
+          </friction>
+          <bounce>
+            <restitution_coefficient>0</restitution_coefficient>
+            <threshold>1e+06</threshold>
+          </bounce>
+          <contact>
+            <collide_without_contact>0</collide_without_contact>
+            <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+            <collide_bitmask>1</collide_bitmask>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0</min_depth>
+            </ode>
+            <bullet>
+              <split_impulse>1</split_impulse>
+              <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+            </bullet>
+          </contact>
+        </surface>
+      </collision>
+    </link>
+    <link name='link_4'>
+      <inertial>
+        <mass>0</mass>
+        <inertia>
+          <ixx>0</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0</iyy>
+          <iyz>0</iyz>
+          <izz>0</izz>
+        </inertia>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+      </inertial>
+      <pose frame=''>0 -0 0.15 -0.000193 0.000381 0</pose>
+      <gravity>1</gravity>
+      <self_collide>0</self_collide>
+      <kinematic>0</kinematic>
+      <enable_wind>0</enable_wind>
+      <visual name='visual'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <cylinder>
+            <radius>0.075</radius>
+            <length>0.01</length>
+          </cylinder>
+        </geometry>
+        <material>
+          <lighting>1</lighting>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <shader type='pixel'>
+            <normal_map>__default__</normal_map>
+          </shader>
+          <ambient>1 0.3 0.3 1</ambient>
+          <diffuse>0.7 0.7 0.7 1</diffuse>
+          <specular>0.01 0.01 0.01 1</specular>
+          <emissive>0 0 0 1</emissive>
+        </material>
+        <transparency>0</transparency>
+        <cast_shadows>1</cast_shadows>
+      </visual>
+      <collision name='collision'>
+        <laser_retro>0</laser_retro>
+        <max_contacts>10</max_contacts>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <cylinder>
+            <radius>0.075</radius>
+            <length>0.01</length>
+          </cylinder>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>1</mu>
+              <mu2>1</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0</slip1>
+              <slip2>0</slip2>
+            </ode>
+            <torsional>
+              <coefficient>1</coefficient>
+              <patch_radius>0</patch_radius>
+              <surface_radius>0</surface_radius>
+              <use_patch_radius>1</use_patch_radius>
+              <ode>
+                <slip>0</slip>
+              </ode>
+            </torsional>
+          </friction>
+          <bounce>
+            <restitution_coefficient>0</restitution_coefficient>
+            <threshold>1e+06</threshold>
+          </bounce>
+          <contact>
+            <collide_without_contact>0</collide_without_contact>
+            <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+            <collide_bitmask>1</collide_bitmask>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0</min_depth>
+            </ode>
+            <bullet>
+              <split_impulse>1</split_impulse>
+              <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+            </bullet>
+          </contact>
+        </surface>
+      </collision>
+    </link>
+    <joint name='link_3_JOINT_0' type='fixed'>
+      <parent>link_3</parent>
+      <child>link_4</child>
+      <pose frame=''>0 0 0 0 -0 0</pose>
+      <physics>
+        <ode>
+          <limit>
+            <cfm>0</cfm>
+            <erp>0.2</erp>
+          </limit>
+          <suspension>
+            <cfm>0</cfm>
+            <erp>0.2</erp>
+          </suspension>
+        </ode>
+      </physics>
+    </joint>
+    <static>0</static>
+    <allow_auto_disable>1</allow_auto_disable>
+  </model>
+</sdf>

src/rrrobot_ws/src/gazebo_models/square_trash_can/model.config

@@ -0,0 +1,11 @@
+<?xml version="1.0" ?>
+<model>
+    <name>square_trash_can</name>
+    <version>1.0</version>
+    <sdf version="1.6">model.sdf</sdf>
+    <author>
+        <name></name>
+        <email></email>
+    </author>
+    <description></description>
+</model>

src/rrrobot_ws/src/gazebo_models/square_trash_can/model.sdf

@@ -0,0 +1,582 @@
+<?xml version='1.0'?>
+<sdf version='1.6'>
+  <model name='square_trash_can'>
+    <link name='link_1'>
+      <inertial>
+        <mass>1</mass>
+        <inertia>
+          <ixx>0.166667</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.166667</iyy>
+          <iyz>0</iyz>
+          <izz>0.166667</izz>
+        </inertia>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+      </inertial>
+      <pose frame=''>-0 0.25 0.082 0 -0 0</pose>
+      <gravity>1</gravity>
+      <self_collide>0</self_collide>
+      <kinematic>0</kinematic>
+      <enable_wind>0</enable_wind>
+      <visual name='visual'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.5 0.02 0.8</size>
+          </box>
+        </geometry>
+        <material>
+          <lighting>1</lighting>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <shader type='pixel'>
+            <normal_map>__default__</normal_map>
+          </shader>
+          <ambient>0.3 0.3 0.3 1</ambient>
+          <diffuse>0.7 0.7 0.7 1</diffuse>
+          <specular>0.01 0.01 0.01 1</specular>
+          <emissive>0 0 0 1</emissive>
+        </material>
+        <transparency>0</transparency>
+        <cast_shadows>1</cast_shadows>
+      </visual>
+      <collision name='collision'>
+        <laser_retro>0</laser_retro>
+        <max_contacts>10</max_contacts>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.5 0.02 0.8</size>
+          </box>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>1</mu>
+              <mu2>1</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0</slip1>
+              <slip2>0</slip2>
+            </ode>
+            <torsional>
+              <coefficient>1</coefficient>
+              <patch_radius>0</patch_radius>
+              <surface_radius>0</surface_radius>
+              <use_patch_radius>1</use_patch_radius>
+              <ode>
+                <slip>0</slip>
+              </ode>
+            </torsional>
+          </friction>
+          <bounce>
+            <restitution_coefficient>0</restitution_coefficient>
+            <threshold>1e+06</threshold>
+          </bounce>
+          <contact>
+            <collide_without_contact>0</collide_without_contact>
+            <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+            <collide_bitmask>1</collide_bitmask>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0</min_depth>
+            </ode>
+            <bullet>
+              <split_impulse>1</split_impulse>
+              <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+            </bullet>
+          </contact>
+        </surface>
+      </collision>
+    </link>
+    <link name='link_1_clone'>
+      <inertial>
+        <mass>1</mass>
+        <inertia>
+          <ixx>0.166667</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.166667</iyy>
+          <iyz>0</iyz>
+          <izz>0.166667</izz>
+        </inertia>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+      </inertial>
+      <pose frame=''>-0 -0.25 0.082 0 -0 0</pose>
+      <gravity>1</gravity>
+      <self_collide>0</self_collide>
+      <kinematic>0</kinematic>
+      <enable_wind>0</enable_wind>
+      <visual name='visual'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.5 0.02 0.8</size>
+          </box>
+        </geometry>
+        <material>
+          <lighting>1</lighting>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <shader type='pixel'>
+            <normal_map>__default__</normal_map>
+          </shader>
+          <ambient>0.3 0.3 0.3 1</ambient>
+          <diffuse>0.7 0.7 0.7 1</diffuse>
+          <specular>0.01 0.01 0.01 1</specular>
+          <emissive>0 0 0 1</emissive>
+        </material>
+        <transparency>0</transparency>
+        <cast_shadows>1</cast_shadows>
+      </visual>
+      <collision name='collision'>
+        <laser_retro>0</laser_retro>
+        <max_contacts>10</max_contacts>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.5 0.02 0.8</size>
+          </box>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>1</mu>
+              <mu2>1</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0</slip1>
+              <slip2>0</slip2>
+            </ode>
+            <torsional>
+              <coefficient>1</coefficient>
+              <patch_radius>0</patch_radius>
+              <surface_radius>0</surface_radius>
+              <use_patch_radius>1</use_patch_radius>
+              <ode>
+                <slip>0</slip>
+              </ode>
+            </torsional>
+          </friction>
+          <bounce>
+            <restitution_coefficient>0</restitution_coefficient>
+            <threshold>1e+06</threshold>
+          </bounce>
+          <contact>
+            <collide_without_contact>0</collide_without_contact>
+            <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+            <collide_bitmask>1</collide_bitmask>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0</min_depth>
+            </ode>
+            <bullet>
+              <split_impulse>1</split_impulse>
+              <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+            </bullet>
+          </contact>
+        </surface>
+      </collision>
+    </link>
+    <link name='link_1_clone_0'>
+      <inertial>
+        <mass>1</mass>
+        <inertia>
+          <ixx>0.166667</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.166667</iyy>
+          <iyz>0</iyz>
+          <izz>0.166667</izz>
+        </inertia>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+      </inertial>
+      <pose frame=''>0.25 0 0.082 0 -0 0</pose>
+      <gravity>1</gravity>
+      <self_collide>0</self_collide>
+      <kinematic>0</kinematic>
+      <enable_wind>0</enable_wind>
+      <visual name='visual'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.02 0.5 0.8</size>
+          </box>
+        </geometry>
+        <material>
+          <lighting>1</lighting>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <shader type='pixel'>
+            <normal_map>__default__</normal_map>
+          </shader>
+          <ambient>0.3 0.3 0.3 1</ambient>
+          <diffuse>0.7 0.7 0.7 1</diffuse>
+          <specular>0.01 0.01 0.01 1</specular>
+          <emissive>0 0 0 1</emissive>
+        </material>
+        <transparency>0</transparency>
+        <cast_shadows>1</cast_shadows>
+      </visual>
+      <collision name='collision'>
+        <laser_retro>0</laser_retro>
+        <max_contacts>10</max_contacts>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.02 0.5 0.8</size>
+          </box>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>1</mu>
+              <mu2>1</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0</slip1>
+              <slip2>0</slip2>
+            </ode>
+            <torsional>
+              <coefficient>1</coefficient>
+              <patch_radius>0</patch_radius>
+              <surface_radius>0</surface_radius>
+              <use_patch_radius>1</use_patch_radius>
+              <ode>
+                <slip>0</slip>
+              </ode>
+            </torsional>
+          </friction>
+          <bounce>
+            <restitution_coefficient>0</restitution_coefficient>
+            <threshold>1e+06</threshold>
+          </bounce>
+          <contact>
+            <collide_without_contact>0</collide_without_contact>
+            <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+            <collide_bitmask>1</collide_bitmask>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0</min_depth>
+            </ode>
+            <bullet>
+              <split_impulse>1</split_impulse>
+              <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+            </bullet>
+          </contact>
+        </surface>
+      </collision>
+    </link>
+    <link name='link_1_clone_0_clone'>
+      <inertial>
+        <mass>1</mass>
+        <inertia>
+          <ixx>0.166667</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.166667</iyy>
+          <iyz>0</iyz>
+          <izz>0.166667</izz>
+        </inertia>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+      </inertial>
+      <pose frame=''>-0.25 0 0.082 0 -0 0</pose>
+      <gravity>1</gravity>
+      <self_collide>0</self_collide>
+      <kinematic>0</kinematic>
+      <enable_wind>0</enable_wind>
+      <visual name='visual'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.02 0.5 0.8</size>
+          </box>
+        </geometry>
+        <material>
+          <lighting>1</lighting>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <shader type='pixel'>
+            <normal_map>__default__</normal_map>
+          </shader>
+          <ambient>0.3 0.3 0.3 1</ambient>
+          <diffuse>0.7 0.7 0.7 1</diffuse>
+          <specular>0.01 0.01 0.01 1</specular>
+          <emissive>0 0 0 1</emissive>
+        </material>
+        <transparency>0</transparency>
+        <cast_shadows>1</cast_shadows>
+      </visual>
+      <collision name='collision'>
+        <laser_retro>0</laser_retro>
+        <max_contacts>10</max_contacts>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.02 0.5 0.8</size>
+          </box>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>1</mu>
+              <mu2>1</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0</slip1>
+              <slip2>0</slip2>
+            </ode>
+            <torsional>
+              <coefficient>1</coefficient>
+              <patch_radius>0</patch_radius>
+              <surface_radius>0</surface_radius>
+              <use_patch_radius>1</use_patch_radius>
+              <ode>
+                <slip>0</slip>
+              </ode>
+            </torsional>
+          </friction>
+          <bounce>
+            <restitution_coefficient>0</restitution_coefficient>
+            <threshold>1e+06</threshold>
+          </bounce>
+          <contact>
+            <collide_without_contact>0</collide_without_contact>
+            <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+            <collide_bitmask>1</collide_bitmask>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0</min_depth>
+            </ode>
+            <bullet>
+              <split_impulse>1</split_impulse>
+              <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+            </bullet>
+          </contact>
+        </surface>
+      </collision>
+    </link>
+    <link name='link_1_clone_0_clone_0'>
+      <inertial>
+        <mass>5000</mass>
+        <inertia>
+          <ixx>104.333</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>104.333</iyy>
+          <iyz>0</iyz>
+          <izz>208.333</izz>
+        </inertia>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+      </inertial>
+      <pose frame=''>-0 -0 -0.328 0 -0 0</pose>
+      <gravity>1</gravity>
+      <self_collide>0</self_collide>
+      <kinematic>0</kinematic>
+      <enable_wind>0</enable_wind>
+      <visual name='visual'>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.5 0.5 0.02</size>
+          </box>
+        </geometry>
+        <material>
+          <lighting>1</lighting>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+          <shader type='pixel'>
+            <normal_map>__default__</normal_map>
+          </shader>
+          <ambient>0.3 0.3 0.3 1</ambient>
+          <diffuse>0.7 0.7 0.7 1</diffuse>
+          <specular>0.01 0.01 0.01 1</specular>
+          <emissive>0 0 0 1</emissive>
+        </material>
+        <transparency>0</transparency>
+        <cast_shadows>1</cast_shadows>
+      </visual>
+      <collision name='collision'>
+        <laser_retro>0</laser_retro>
+        <max_contacts>10</max_contacts>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.5 0.5 0.02</size>
+          </box>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>1</mu>
+              <mu2>1</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0</slip1>
+              <slip2>0</slip2>
+            </ode>
+            <torsional>
+              <coefficient>1</coefficient>
+              <patch_radius>0</patch_radius>
+              <surface_radius>0</surface_radius>
+              <use_patch_radius>1</use_patch_radius>
+              <ode>
+                <slip>0</slip>
+              </ode>
+            </torsional>
+          </friction>
+          <bounce>
+            <restitution_coefficient>0</restitution_coefficient>
+            <threshold>1e+06</threshold>
+          </bounce>
+          <contact>
+            <collide_without_contact>0</collide_without_contact>
+            <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+            <collide_bitmask>1</collide_bitmask>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0</min_depth>
+            </ode>
+            <bullet>
+              <split_impulse>1</split_impulse>
+              <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+            </bullet>
+          </contact>
+        </surface>
+      </collision>
+    </link>
+    <joint name='link_1_JOINT_0' type='fixed'>
+      <parent>link_1</parent>
+      <child>link_1_clone_0</child>
+      <pose frame=''>0 0 0 0 -0 0</pose>
+      <physics>
+        <ode>
+          <limit>
+            <cfm>0</cfm>
+            <erp>0.2</erp>
+          </limit>
+          <suspension>
+            <cfm>0</cfm>
+            <erp>0.2</erp>
+          </suspension>
+        </ode>
+      </physics>
+    </joint>
+    <joint name='link_1_JOINT_4' type='fixed'>
+      <parent>link_1</parent>
+      <child>link_1_clone_0_clone_0</child>
+      <pose frame=''>0 0 0 0 -0 0</pose>
+      <physics>
+        <ode>
+          <limit>
+            <cfm>0</cfm>
+            <erp>0.2</erp>
+          </limit>
+          <suspension>
+            <cfm>0</cfm>
+            <erp>0.2</erp>
+          </suspension>
+        </ode>
+      </physics>
+    </joint>
+    <joint name='link_1_clone_0_JOINT_1' type='fixed'>
+      <parent>link_1_clone_0</parent>
+      <child>link_1_clone</child>
+      <pose frame=''>0 0 0 0 -0 0</pose>
+      <physics>
+        <ode>
+          <limit>
+            <cfm>0</cfm>
+            <erp>0.2</erp>
+          </limit>
+          <suspension>
+            <cfm>0</cfm>
+            <erp>0.2</erp>
+          </suspension>
+        </ode>
+      </physics>
+    </joint>
+    <joint name='link_1_clone_0_clone_JOINT_3' type='fixed'>
+      <parent>link_1_clone_0_clone</parent>
+      <child>link_1</child>
+      <pose frame=''>0 0 0 0 -0 0</pose>
+      <physics>
+        <ode>
+          <limit>
+            <cfm>0</cfm>
+            <erp>0.2</erp>
+          </limit>
+          <suspension>
+            <cfm>0</cfm>
+            <erp>0.2</erp>
+          </suspension>
+        </ode>
+      </physics>
+    </joint>
+    <joint name='link_1_clone_JOINT_2' type='fixed'>
+      <parent>link_1_clone</parent>
+      <child>link_1_clone_0_clone</child>
+      <pose frame=''>0 0 0 0 -0 0</pose>
+      <physics>
+        <ode>
+          <limit>
+            <cfm>0</cfm>
+            <erp>0.2</erp>
+          </limit>
+          <suspension>
+            <cfm>0</cfm>
+            <erp>0.2</erp>
+          </suspension>
+        </ode>
+      </physics>
+    </joint>
+    <static>0</static>
+    <allow_auto_disable>1</allow_auto_disable>
+  </model>
+</sdf>

src/rrrobot_ws/src/gazebo_models/unit_box/model.config

@@ -0,0 +1,11 @@
+<?xml version="1.0" ?>
+<model>
+    <name>unit_box</name>
+    <version>1.0</version>
+    <sdf version="1.6">model.sdf</sdf>
+    <author>
+        <name></name>
+        <email></email>
+    </author>
+    <description></description>
+</model>

src/rrrobot_ws/src/gazebo_models/unit_box/model.sdf

@@ -0,0 +1,104 @@
+<?xml version='1.0'?>
+<sdf version='1.6'>
+  <model name='unit_box'>
+    <link name='link'>
+      <inertial>
+        <mass>0.208095</mass>
+        <inertia>
+          <ixx>0.0180922</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.0180922</iyy>
+          <iyz>0</iyz>
+          <izz>0.0346825</izz>
+        </inertia>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+      </inertial>
+      <self_collide>0</self_collide>
+      <enable_wind>0</enable_wind>
+      <kinematic>0</kinematic>
+      <pose frame=''>0 0 0 0 -0 0</pose>
+      <gravity>1</gravity>
+      <visual name='visual'>
+        <geometry>
+          <box>
+            <size>0.05 0.05 0.05</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <name>Gazebo/Grey</name>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+          </script>
+          <shader type='pixel'>
+            <normal_map>__default__</normal_map>
+          </shader>
+          <ambient>1 0 0 1</ambient>
+          <diffuse>0.7 0.7 0.7 1</diffuse>
+          <specular>0.01 0.01 0.01 1</specular>
+          <emissive>0 0 0 1</emissive>
+        </material>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <transparency>0</transparency>
+        <cast_shadows>1</cast_shadows>
+      </visual>
+      <collision name='collision'>
+        <laser_retro>0</laser_retro>
+        <max_contacts>10</max_contacts>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.05 0.05 0.05</size>
+          </box>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>1</mu>
+              <mu2>1</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0</slip1>
+              <slip2>0</slip2>
+            </ode>
+            <torsional>
+              <coefficient>1</coefficient>
+              <patch_radius>0</patch_radius>
+              <surface_radius>0</surface_radius>
+              <use_patch_radius>1</use_patch_radius>
+              <ode>
+                <slip>0</slip>
+              </ode>
+            </torsional>
+          </friction>
+          <bounce>
+            <restitution_coefficient>0</restitution_coefficient>
+            <threshold>1e+06</threshold>
+          </bounce>
+          <contact>
+            <collide_without_contact>0</collide_without_contact>
+            <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+            <collide_bitmask>1</collide_bitmask>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0</min_depth>
+            </ode>
+            <bullet>
+              <split_impulse>1</split_impulse>
+              <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+            </bullet>
+          </contact>
+        </surface>
+      </collision>
+    </link>
+    <static>0</static>
+    <allow_auto_disable>1</allow_auto_disable>
+  </model>
+</sdf>

src/rrrobot_ws/src/rrrobot/CMakeLists.txt

@@ -0,0 +1,163 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(rrrobot)
+
+set(CMAKE_BUILD_TYPE Debug)
+
+find_package(catkin REQUIRED COMPONENTS
+  osrf_gear
+  roscpp
+  sensor_msgs
+  std_srvs
+  trajectory_msgs
+  std_msgs
+  geometry_msgs
+  gazebo_ros
+  message_generation
+  pcl_ros
+  pcl_conversions
+)
+
+find_package(gazebo REQUIRED)
+find_package(PCL 1.8 REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(orocos_kdl REQUIRED)
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+## Generate messages in the 'msg' folder
+add_message_files(
+  FILES
+  model_insertion.msg
+  arm_command.msg
+)
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+  geometry_msgs
+)
+
+catkin_package(
+  INCLUDE_DIRS
+#  LIBRARIES simulation_env
+  CATKIN_DEPENDS roscpp std_msgs geometry_msgs gazebo_ros
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${roscpp_INCLUDE_DIRS}
+  ${std_msgs_INCLUDE_DIRS}
+  ${geometry_msgs_INCLUDE_DIRS}
+  ${GAZEBO_INCLUDE_DIRS}
+  ${PCL_INCLUDE_DIRS}
+  ${Eigen3_INCLUDE_DIRS}
+)
+
+add_library(insert_model SHARED
+  src/model_insertion_plugin.cpp
+)
+add_dependencies(insert_model ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+
+## Declare a C++ executable
+add_executable(rrrobot_node src/rrrobot_node.cpp)
+add_dependencies(rrrobot_node ${catkin_EXPORTED_TARGETS} rrrobot_generate_messages_cpp)
+target_link_libraries(rrrobot_node ${catkin_LIBRARIES})
+
+add_executable(arm_controller_node src/arm_controller_node.cpp  src/arm_representation.cpp)
+add_dependencies(arm_controller_node ${catkin_EXPORTED_TARGETS})
+target_link_libraries(arm_controller_node ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES})
+
+add_executable(test_arm test/test_arm.cpp src/arm_representation.cpp)
+add_dependencies(test_arm ${catkin_EXPORTED_TARGETS})
+target_link_libraries(test_arm ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES})
+
+add_executable(depth_camera_node src/depth_camera_node.cpp)
+add_dependencies(depth_camera_node ${catkin_EXPORTED_TARGETS})
+target_link_libraries(depth_camera_node ${catkin_LIBRARIES} ${PCL_LIBRARIES})
+
+add_executable(test_insert_object test/test_insert_object.cpp)
+add_executable(object_spawner_node src/object_spawner_node.cpp)
+
+add_dependencies(test_insert_object insert_model)
+add_dependencies(object_spawner_node insert_model)
+
+target_link_libraries(test_insert_object
+  ${catkin_LIBRARIES}
+)
+target_link_libraries(object_spawner_node
+  ${catkin_LIBRARIES}
+)
+
+target_link_libraries(insert_model
+  ${catkin_LIBRARIES}
+  ${GAZEBO_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+# install(PROGRAMS
+#   script/rrrobot_node.py
+#   script/tf2_example.py
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+install(TARGETS rrrobot_node
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(TARGETS arm_controller_node
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(TARGETS depth_camera_node
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#  config/sample_gear_conf.yaml
+#  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_ariac_example.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

src/rrrobot_ws/src/rrrobot/config/rrrobot.yaml

@@ -0,0 +1,14 @@
+# Competition configuration options
+options:
+  insert_models_over_bins: true     # Whether or not to insert the models that are specified in models_over_bins
+  spawn_extra_models: true          # Whether or not to spawn the models that are specified in models_to_spawn
+  gazebo_state_logging: true        # Whether or not to generate a gazebo state log
+  belt_population_cycles: 5         # How many cycles to spawn parts on the conveyor
+  model_type_aliases:               # Aliases for model types which can be used in the configuration file
+    order_part1: piston_rod_part    # Wherever 'order_part1' is used in the configuration file, use 'piston_rod_part'
+    order_part2: gear_part
+    order_part3: pulley_part
+  visualize_drop_regions: false     # Whether or not to visualize drop regions (world frame only)
+
+time_limit: 600                      # Maximum time allowed for the trial once started, in seconds
+random_seed: 1                       # Seed for the pseudo random number generator (used to randomize model names)

src/rrrobot_ws/src/rrrobot/config/rrrobot_sensors.yaml

@@ -0,0 +1,6 @@
+sensors:
+    depth_camera_1:
+      type: depth_camera
+      pose:
+        xyz: [1.0, 1.2, 1.4]
+        rpy: [0, 1.21, 0]

src/rrrobot_ws/src/rrrobot/include/arm_representation.h

@@ -0,0 +1,59 @@
+// arm_representation.h
+
+#include <algorithm>
+#include <vector>
+#include <string>
+#include <memory>
+
+#include <ros/ros.h>
+
+#include <std_msgs/Float32.h>
+#include <std_msgs/String.h>
+#include <std_srvs/Trigger.h>
+
+#include <osrf_gear/VacuumGripperControl.h>
+#include <trajectory_msgs/JointTrajectory.h>
+
+#include "topic_names.h"
+#include "rrrobot/arm_command.h"
+
+#include <kdl/chain.hpp>
+#include <kdl/segment.hpp>
+#include <kdl/rigidbodyinertia.hpp>
+#include <kdl/rotationalinertia.hpp>
+#include <kdl/joint.hpp>
+#include <kdl/frames.hpp>
+#include <LinearMath/btTransform.h>
+#include <kdl/chainfksolver.hpp>
+#include <kdl/chainiksolverpos_lma.hpp>
+#include <kdl/chainfksolverpos_recursive.hpp>
+#include <kdl/chainidsolver_recursive_newton_euler.hpp>
+
+#include <fstream>
+#include <iostream>
+#include <string>
+
+// using namespace std;
+using std::string;
+
+class ArmRepresentation
+{
+public:
+    ArmRepresentation(const KDL::Frame &base_pose = KDL::Frame(KDL::Rotation::Quaternion(0, 0, 0, 1), KDL::Vector(0.3, 0.92, 0.9))); //KDL::Frame(KDL::Vector(0.3, 0.92, 1)));
+
+    int calculateForwardKinematics(const KDL::JntArray &joint_positions, KDL::Frame &end_effector_pose, int joint_nbr = -1);
+
+    int calculateInverseKinematics(const std::vector<double> &cur_configuration,
+                                   const KDL::Frame &desired_end_effector_pose,
+                                   KDL::JntArray &final_joint_configuration);
+
+    std::vector<string> get_joint_names();
+
+    KDL::Chain *getChain();
+
+private:
+    KDL::Chain chain;
+
+    // KDL::ChainFkSolverPos_recursive fk_solver;
+    // KDL::ChainIkSolverPos_LMA ik_solver;
+};

src/rrrobot_ws/src/rrrobot/include/topic_names.h

@@ -0,0 +1,18 @@
+// topic_names.h
+
+// COMPUTER VISION
+#define CV_CLASSIFICATION_CHANNEL   "/cv_model"                     // CV Model classifications
+
+// DEPTH CAMERA
+#define DESIRED_GRASP_POSE_CHANNEL  "/desired_grasp_pose"           // Pose determined from depth camera
+
+// ARM CONTROLLER
+#define ARM_DESTINATION_CHANNEL     "/arm_controller/destination"   // Sent from rrrobot_node to arm_controller when depth camera sees item
+#define ARM_COMMAND_CHANNEL         "/ariac/arm1/arm/command"       // Send joint trajectories to arm
+#define ARM_JOINT_STATES_CHANNEL    "/ariac/arm1/joint_states"      // See arm's current joint states
+#define GRIPPER_STATE_CHANNEL       "/ariac/arm1/gripper/state"     // Get the state of the gripper
+#define GRIPPER_CONTROL_CHANNEL     "/ariac/arm1/gripper/control"   // Turn gripper on or off
+
+// COMPETITION
+#define START_COMPETITION_CHANNEL   "/ariac/start_competition"      // Start ARIAC competition
+#define CONVEYOR_POWER_CHANNEL      "/ariac/conveyor/control"       // Turn conveyor belt on or off

src/rrrobot_ws/src/rrrobot/launch/rrrobot.launch

@@ -0,0 +1,28 @@
+<launch>
+  <arg name="verbose" default="false" />
+  <arg unless="$(arg verbose)" name="verbose_args" value="" />
+  <arg     if="$(arg verbose)" name="verbose_args" value="--verbose" />
+
+  <arg name="state_logging" default="false" />
+  <arg unless="$(arg state_logging)" name="state_logging_args" value="" />
+  <arg     if="$(arg state_logging)" name="state_logging_args" value="--state-logging=true" />
+
+  <arg name="no_gui" default="false" />
+  <arg unless="$(arg no_gui)" name="gui_args" value="" />
+  <arg     if="$(arg no_gui)" name="gui_args" value="--no-gui" />
+
+  <arg name="fill_demo_shipment" default="false" />
+  <arg unless="$(arg fill_demo_shipment)" name="fill_demo_shipment_args" value="" />
+  <arg     if="$(arg fill_demo_shipment)" name="fill_demo_shipment_args" value="--fill-demo-shipment" />
+
+  <node name="ariac_sim" pkg="osrf_gear" type="gear.py"
+        args="--development-mode
+          $(arg verbose_args)
+          $(arg state_logging_args)
+          $(arg gui_args)
+          $(arg fill_demo_shipment_args)
+          --visualize-sensor-views
+          -f /app/rrrobot_ws/src/rrrobot/config/rrrobot.yaml
+          /app/rrrobot_ws/src/rrrobot/config/rrrobot_sensors.yaml
+          " required="true" output="screen" />
+</launch>

src/rrrobot_ws/src/rrrobot/msg/arm_command.msg

@@ -0,0 +1,2 @@
+geometry_msgs/Pose grab_location
+geometry_msgs/Pose drop_location

src/rrrobot_ws/src/rrrobot/msg/model_insertion.msg

@@ -0,0 +1,2 @@
+string model_name
+geometry_msgs/Pose pose

src/rrrobot_ws/src/rrrobot/package.xml

@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<package format="2">
+    <name>rrrobot</name>
+    <version>1.0.0</version>
+    <description>RRRobot package</description>
+    <maintainer email="balajsra@umich.edu">Sravan Balaji</maintainer>
+    <maintainer email="chenxgu@umich.edu">Chenxi Gu</maintainer>
+    <maintainer email="thejakej@umich.edu">Jake Johnson</maintainer>
+    <maintainer email="dwitcpa@umich.edu">Derek Witcpalek</maintainer>
+
+    <license>MIT</license>
+
+    <buildtool_depend>catkin</buildtool_depend>
+
+    <depend>osrf_gear</depend>
+    <depend>roscpp</depend>
+    <depend>sensor_msgs</depend>
+    <depend>std_srvs</depend>
+    <depend>trajectory_msgs</depend>
+    <depend>std_msgs</depend>
+    <depend>geometry_msgs</depend>
+    <depend>gazebo_ros</depend>
+    <depend>message_generation</depend>
+    <depend>pcl_ros</depend>
+    <depend>pcl_conversion</depend>
+    <depend>gazebo</depend>
+    <depend>PCL</depend>
+    <depend>Eigen3</depend>
+    <depend>orocos_kdl</depend>
+    <exec_depend>message_runtime</exec_depend>
+
+</package>

src/rrrobot_ws/src/rrrobot/scripts/arm_home_position.sh

@@ -0,0 +1,8 @@
+#!/bin/bash
+
+rostopic pub /ariac/arm1/arm/command trajectory_msgs/JointTrajectory    "{joint_names: \
+        ['linear_arm_actuator_joint',  'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'], \
+     points: [ \
+{time_from_start: {secs: 5}, \
+        positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}, \
+]}" -1

src/rrrobot_ws/src/rrrobot/scripts/arm_pub_test.sh

@@ -0,0 +1,25 @@
+#!/bin/bash
+
+source /app/rrrobot_ws/devel/setup.bash
+
+rostopic pub /arm_controller/destination rrrobot/arm_command "grab_location:
+  position:
+    x: 1.22
+    y: 1.22
+    z: 0.9725
+  orientation:
+    x: 0.0
+    y: 0.707
+    z: 0.0
+    w: 0.707
+drop_location:
+  position:
+    x: -0.3
+    y: 0.383
+    z: 1.0
+  orientation:
+    x: 0.0
+    y: 0.707
+    z: 0.0
+    w: 0.707" \
+-1

src/rrrobot_ws/src/rrrobot/scripts/arm_pub_test_continuous.sh

@@ -0,0 +1,23 @@
+#!/bin/bash
+
+#rostopic pub /target_pose geometry_msgs/Pose '{position: {x: 0.875, y: 0.75, z: 1.5}, orientation: {x: 0, y: 0, z: 0, w: 0}}'
+rostopic pub /arm_controller/destination rrrobot/arm_command "grab_location:
+  position:
+    x: 1.2
+    y: 0.2
+    z: 1.0
+  orientation:
+    x: 0.0
+    y: 0.0
+    z: 0.0
+    w: 0.0
+drop_location:
+  position:
+    x: -0.3
+    y: 1.15
+    z: 1.5
+  orientation:
+    x: 0.0
+    y: 0.0
+    z: 0.0
+    w: 0.0"

src/rrrobot_ws/src/rrrobot/scripts/arm_test.sh

@@ -0,0 +1,8 @@
+#!/bin/bash
+
+cd /app/rrrobot_ws
+# catkin_make clean
+catkin_make
+# catkin_make install
+source devel/setup.bash
+rosrun rrrobot arm_controller_node

src/rrrobot_ws/src/rrrobot/scripts/rrrobot_run.sh

@@ -0,0 +1,15 @@
+#!/bin/bash
+
+cd /app/rrrobot_ws/
+
+# catkin_make clean
+catkin_make
+# catkin_make install
+
+cd /app/rrrobot_ws/src/rrrobot/scripts
+
+sudo cp ../launch/rrrobot.launch /opt/ros/melodic/share/osrf_gear/launch
+sudo cp ../../../world/gear.py /opt/ros/melodic/lib/osrf_gear/gear.py
+sudo cp ../../../world/gear.world.template /opt/ros/melodic/share/osrf_gear/worlds/gear.world.template
+
+roslaunch osrf_gear rrrobot.launch

src/rrrobot_ws/src/rrrobot/scripts/rrrobot_run_no_build.sh

@@ -0,0 +1,7 @@
+#!/bin/bash
+
+sudo cp ../launch/rrrobot.launch /opt/ros/melodic/share/osrf_gear/launch
+sudo cp ../../../world/gear.py /opt/ros/melodic/lib/osrf_gear/gear.py
+sudo cp ../../../world/gear.world.template /opt/ros/melodic/share/osrf_gear/worlds/gear.world.template
+
+roslaunch osrf_gear rrrobot.launch

src/rrrobot_ws/src/rrrobot/scripts/sample_run.sh

@@ -0,0 +1,62 @@
+#!/bin/bash
+
+# Start Competition
+rosservice call /ariac/start_competition
+
+# Wait
+sleep 2
+
+# Move arm1 over a gasket part
+rostopic pub /ariac/arm1/arm/command trajectory_msgs/JointTrajectory    "{joint_names: \
+        ['linear_arm_actuator_joint',  'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'], \
+     points: [ \
+{time_from_start: {secs: 2}, \
+        positions: [0.15, 3.14,  -1.570,  2.14, 3.1, -1.59, 0.126]}, \
+{time_from_start: {secs: 4}, \
+        positions: [-0.35, 3.14,  -0.6,  2.3, 3.0, -1.59, 0.126]}, \
+{time_from_start: {secs: 6}, \
+        positions: [-0.35, 3.14,  -0.5,  2.3, 3.05, -1.59, 0.126]}, \
+]}" -1
+
+# Wait for arm to move
+sleep 8
+
+# Turn gripper suction on
+rosservice call /ariac/arm1/gripper/control "enable: true"
+
+# Wait for suction
+sleep 2
+
+# Move part to AGV1's tray
+rostopic pub /ariac/arm1/arm/command trajectory_msgs/JointTrajectory    "{joint_names: \
+        ['linear_arm_actuator_joint',  'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'], \
+     points: [ \
+{time_from_start: {secs: 2}, \
+        positions: [0.0, 3.14,  -1.570,  2.14, 3.27, -1.51, 0.0]}, \
+{time_from_start: {secs: 5}, \
+        positions: [1.0, 1.85,  0,  -0.38, 1.57, -1.51, 0.00]}, \
+{time_from_start: {secs: 7}, \
+        positions: [1.0, 1.507,  0,  -0.38, 0.38, -1.51, 0.00]}, \
+{time_from_start: {secs: 10}, \
+        positions: [1.18, 1.507,  0.38,  -0.38, 1.55, 1.75, 0.127]}, \
+]}" -1
+
+# Wait for arm to move
+sleep 12
+
+# Turn gripper suction off
+rosservice call /ariac/arm1/gripper/control "enable: false"
+
+# Wait for suction
+sleep 2
+
+# Return to starting position
+rostopic pub /ariac/arm1/arm/command trajectory_msgs/JointTrajectory    "{joint_names: \
+        ['linear_arm_actuator_joint',  'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'], \
+     points: [ \
+{time_from_start: {secs: 5}, \
+        positions: [0.0, 3.14,  -1.570,  2.14, 3.27, -1.51, 0.0]}, \
+]}" -1
+
+# Wait for arm to move
+sleep 7

src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp

@@ -0,0 +1,550 @@
+// arm_controller_node.cpp
+
+#include <algorithm>
+#include <vector>
+#include <string>
+#include <memory>
+
+#include <ros/ros.h>
+#include <ros/console.h>
+
+#include <std_msgs/Float32.h>
+#include <std_msgs/String.h>
+#include <std_srvs/Trigger.h>
+
+#include <osrf_gear/VacuumGripperControl.h>
+#include <trajectory_msgs/JointTrajectory.h>
+#include <sensor_msgs/JointState.h>
+#include <osrf_gear/VacuumGripperState.h>
+
+#include "topic_names.h"
+#include "rrrobot/arm_command.h"
+#include "arm_representation.h"
+
+#include <kdl/chain.hpp>
+#include <kdl/segment.hpp>
+#include <kdl/rigidbodyinertia.hpp>
+#include <kdl/rotationalinertia.hpp>
+#include <kdl/joint.hpp>
+#include <kdl/frames.hpp>
+#include <LinearMath/btTransform.h>
+#include <kdl/chainfksolver.hpp>
+#include <kdl/chainiksolverpos_lma.hpp>
+#include <kdl/chainfksolverpos_recursive.hpp>
+#include <kdl/chainidsolver_recursive_newton_euler.hpp>
+
+#include <stdlib.h> /* srand, rand */
+#include <time.h>   /* time */
+#include <sstream>
+
+#include <cstdlib>
+#include <math.h>
+
+using namespace std;
+
+class ArmController
+{
+public:
+    // Constructor
+    ArmController(ros::NodeHandle &node, ArmRepresentation *arm_, double time_per_step, int retry_attempts, double item_attach_z_adjustment) : gripper_enabled_(false), item_attached_(false), arm(arm_), time_per_step(time_per_step), retry_attempts(retry_attempts), item_attach_z_adjustment(item_attach_z_adjustment)
+    {
+        arm_joint_trajectory_publisher_ = node.advertise<trajectory_msgs::JointTrajectory>(ARM_COMMAND_CHANNEL, 10);
+
+        gripper_ = node.serviceClient<osrf_gear::VacuumGripperControl>(GRIPPER_CONTROL_CHANNEL);
+
+        arm_current_joint_states_.resize(arm->getChain()->getNrOfJoints(), 0.0);
+
+        /* initialize random seed: */
+        srand(time(NULL));
+    }
+
+    // Receive joint state messages
+    void joint_state_callback(const sensor_msgs::JointState &joint_state_msg)
+    {
+        // Copy joint states to private variable arm_current_joint_states_
+        int nbr_joints = arm->getChain()->getNrOfJoints();
+        vector<string> msg_joint_names = joint_state_msg.name;
+        vector<string> cur_joint_names = arm->get_joint_names();
+        vector<double> position = joint_state_msg.position;
+
+        // Set all values to 0
+        for (int i = 0; i < nbr_joints; ++i)
+        {
+            arm_current_joint_states_[i] = 0.0;
+        }
+
+        // Match up joint names from message to internal joint names order
+        for (size_t i = 0; i < position.size(); ++i)
+        {
+            for (size_t j = 0; j < cur_joint_names.size(); ++j)
+            {
+                if (msg_joint_names[i].compare(cur_joint_names[j]) == 0)
+                {
+                    arm_current_joint_states_[j] = position[i];
+                }
+            }
+        }
+    }
+
+    // Receive gripper state messages
+    void gripper_state_callback(const osrf_gear::VacuumGripperState::ConstPtr &gripper_state_msg)
+    {
+        // Store message states in private variables
+        gripper_enabled_ = gripper_state_msg->enabled;
+        item_attached_ = gripper_state_msg->attached;
+    }
+
+    // Receive message from RRRobot node with desired pose to pick up object from conveyor belt
+    void arm_destination_callback(const rrrobot::arm_command &target_pose)
+    {
+        ROS_INFO("Received target pose");
+        int attempts = 0;
+        int nbr_joints = arm->getChain()->getNrOfJoints();
+        vector<double> positions;
+
+        // Turn on suction
+        while (!gripper_control(true))
+        {
+            if (attempts == retry_attempts)
+            {
+                ROS_ERROR("STEP 1: Suction failed, not going to target");
+                return;
+            }
+
+            ros::Duration(1).sleep();
+            attempts++;
+        }
+
+        ROS_INFO("STEP 1: Suction turned on");
+
+        // Move arm to pickup item from conveyor belt
+        attempts = 1;
+        if (calc_joint_positions(target_pose.grab_location, above_conveyor, positions))
+            send_joint_trajectory(positions, arm_action_phase::bin_to_belt);
+
+        // Check if target has been reached
+        while (!have_reached_target(frame_to_pose(calc_end_effector_pose()), target_pose.grab_location))
+        {
+            if (attempts == retry_attempts)
+            {
+                ROS_ERROR("STEP 2: Did not reach target in allotted time");
+                return;
+            }
+
+            if (calc_joint_positions(target_pose.grab_location, get_randomized_start_state(above_conveyor) /*target_pose.grab_location.position.y)*/, positions))
+                send_joint_trajectory(positions, arm_action_phase::bin_to_belt);
+            attempts++;
+        }
+
+        ROS_INFO("STEP 2: Reached target conveyor belt position");
+
+        attempts = 0;
+        geometry_msgs::Pose target_location = target_pose.grab_location;
+
+        // Wait until object is attached
+        while (!item_attached_)
+        {
+            if (attempts == retry_attempts)
+            {
+                ROS_ERROR("STEP 3: Could not pick up item");
+                return;
+            }
+
+            // If item is not able to attach, adjust z and try again
+            target_location.position.z -= item_attach_z_adjustment; // meters
+            if (calc_joint_positions(target_location, arm_current_joint_states_, positions))
+                send_joint_trajectory(positions, arm_action_phase::item_pickup_adjustment);
+            attempts++;
+        }
+
+        ROS_INFO("STEP 3: Item picked up");
+
+        // Move item to desired position
+        attempts = 1;
+        KDL::Frame end_effector_pose;
+        
+        if (target_pose.drop_location.position.y > 1.0)
+        {
+            positions = bin1;
+            KDL::JntArray pos(positions.size());
+            for (size_t idx = 0; idx < positions.size(); ++idx)
+            {
+                pos(idx) = positions[idx];
+            }
+            arm->calculateForwardKinematics(pos, end_effector_pose);
+        }
+        else
+        {
+            positions = bin2;
+            KDL::JntArray pos(positions.size());
+            for (size_t idx = 0; idx < positions.size(); ++idx)
+            {
+                pos(idx) = positions[idx];
+            }
+            arm->calculateForwardKinematics(pos, end_effector_pose);
+        }
+
+        // Check if target has been reached
+        while (!have_reached_target(frame_to_pose(calc_end_effector_pose()), frame_to_pose(end_effector_pose))) //target_pose.drop_location))
+        {
+            if (attempts == retry_attempts)
+            {
+                ROS_ERROR("STEP 4: Did not reach target in allotted time");
+                return;
+            }
+
+            send_joint_trajectory(positions, arm_action_phase::belt_to_bin);
+            attempts++;
+        }
+
+        ROS_INFO("STEP 4: Reached drop location");
+
+        // Turn off suction
+        attempts = 0;
+        while (!gripper_control(false))
+        {
+            if (attempts == retry_attempts)
+            {
+                ROS_ERROR("STEP 5: Suction not turning off... ¯\\_(ツ)_/¯");
+                return;
+            }
+
+            ros::Duration(1).sleep();
+            attempts++;
+        }
+
+        ROS_INFO("STEP 5: Suction turned off");
+
+        // Wait until object is detached
+        attempts = 0;
+        while (item_attached_)
+        {
+            if (attempts == retry_attempts)
+            {
+                ROS_ERROR("STEP 6: Item not detaching.. ¯\\_(ツ)_/¯");
+            }
+
+            ros::Duration(1).sleep();
+            attempts++;
+        }
+
+        ROS_INFO("STEP 6: Dropped item in bin");
+    }
+
+private:
+    // Private variables
+    bool gripper_enabled_;
+    bool item_attached_;
+    ros::Publisher arm_joint_trajectory_publisher_;
+    ros::ServiceClient gripper_;
+    vector<double> arm_current_joint_states_;
+    ArmRepresentation *arm;
+    enum arm_action_phase
+    {
+        belt_to_bin,
+        bin_to_belt,
+        item_pickup_adjustment
+    };
+    double time_per_step;
+    int retry_attempts;
+    double item_attach_z_adjustment;
+    // Define intermediate joint positions
+    const vector<double> above_conveyor = {0, 0, -M_PI / 2, M_PI / 2, -M_PI / 2, -M_PI / 2, 0};
+    const vector<double> above_bins = {0, M_PI, -M_PI / 2, M_PI / 2, -M_PI / 2, -M_PI / 2, 0};
+    const vector<double> bin1 = {0.05, 0.0, -2.2, -2.2, -0.25, 1.5708, 0.0};
+    const vector<double> bin2 = {-0.7, 0.0, -2.2, -2.2, -0.25, 1.5708, 0.0};
+
+    // Randomize start state to avoid inverse kinematics getting stuck in local minimum
+    vector<double> get_randomized_start_state(const vector<double> &preferred_state)
+    {
+        vector<double> cur;
+        std::stringstream location;
+        location << "Random location: ";
+        // cur.push_back(y);
+        for (size_t pos = 0; pos < arm->getChain()->getNrOfJoints(); ++pos) {
+            int val = rand() % 2;
+            double diff = 0;
+            
+            if (val == 1) {
+                diff = -0.1;
+            }
+            else {
+                diff = 0.1;
+            }
+            
+            cur.push_back(preferred_state[pos] + diff);
+            location << cur[pos] << " ";
+        }
+
+        location << '\n';
+        ROS_INFO(location.str().c_str());
+
+        return cur;
+    }
+
+    // Use inverse kinematics to calculate joint positions to reach desired pose 
+    bool calc_joint_positions(const geometry_msgs::Pose &pose, const vector<double> &start_state, vector<double> &positions)
+    {
+        KDL::Frame desired_end_effector_pose = pose_to_frame(pose);
+        KDL::JntArray final_joint_configuration(arm->getChain()->getNrOfJoints());
+
+        // ROS_INFO("cur_configuration (%i x %i)", cur_configuration.rows(), cur_configuration.columns());
+
+        int error_code = arm->calculateInverseKinematics(start_state, desired_end_effector_pose, final_joint_configuration);
+
+        // Check status of IK and print error message if there is a failure
+        if (error_code != 0)
+        {
+            ROS_ERROR("Inverse Kinematics Failure: %i", error_code);
+        }
+
+        // Convert data attribute (Eigen::VectorXd) of KDL::JntArray to double[] via data() function
+        int nbr_joints = arm->getChain()->getNrOfJoints();
+        Eigen::VectorXd mat = final_joint_configuration.data;
+        positions.clear();
+        std::stringstream pos_str;
+        pos_str << "Calculated joint positions: ";
+        
+        for (size_t idx = 0; idx < arm->getChain()->getNrOfJoints(); ++idx)
+        {
+            positions.push_back(mat[idx]);
+            pos_str << mat[idx] << " ";
+        }
+        
+        pos_str << '\n';
+
+        ROS_INFO(pos_str.str().c_str());
+
+        return (error_code == 0);
+    }
+
+    // Use forward kinematics to calculate end effector pose from current joint states
+    KDL::Frame calc_end_effector_pose()
+    {
+        int num_joints = arm->getChain()->getNrOfJoints();
+
+        KDL::Frame end_effector_pose;
+        KDL::JntArray joint_positions(num_joints);
+
+        // Copy current joint states values into JntArray
+        for (int i = 0; i < num_joints; ++i)
+        {
+            joint_positions(i) = arm_current_joint_states_[i];
+        }
+
+        int error_code = arm->calculateForwardKinematics(joint_positions, end_effector_pose);
+
+        // Check status of FK and do something if there is a failure
+        if (error_code != 0)
+        {
+            ROS_ERROR("Forward Kinematics Failure: %i", error_code);
+        }
+
+        return end_effector_pose;
+    }
+
+    // Convert from Pose to KDL::Frame
+    KDL::Frame pose_to_frame(const geometry_msgs::Pose &pose)
+    {
+        double p_x = pose.position.x;
+        double p_y = pose.position.y;
+        double p_z = pose.position.z;
+
+        double q_x = pose.orientation.x;
+        double q_y = pose.orientation.y;
+        double q_z = pose.orientation.z;
+        double q_w = pose.orientation.w;
+
+        KDL::Rotation rot(KDL::Rotation::Quaternion(q_x, q_y, q_z, q_w));
+        KDL::Vector pos(p_x, p_y, p_z);
+        return KDL::Frame(rot, pos);
+    }
+
+    // Convert from KDL::Frame to Pose
+    geometry_msgs::Pose frame_to_pose(const KDL::Frame &frame)
+    {
+        double p_x = frame.p.x();
+        double p_y = frame.p.y();
+        double p_z = frame.p.z();
+
+        double q_x, q_y, q_z, q_w;
+        frame.M.GetQuaternion(q_x, q_y, q_z, q_w);
+
+        geometry_msgs::Pose pose;
+
+        pose.position.x = p_x;
+        pose.position.y = p_y;
+        pose.position.z = p_z;
+
+        pose.orientation.x = q_x;
+        pose.orientation.y = q_y;
+        pose.orientation.z = q_z;
+        pose.orientation.w = q_w;
+
+        return pose;
+    }
+
+    // Send desired joint states to joint controller 
+    void send_joint_trajectory(const vector<double> &target, arm_action_phase phase)
+    {
+        // Declare JointTrajectory message
+        trajectory_msgs::JointTrajectory msg;
+
+        // Fill the names of the joints to be controlled
+        msg.joint_names = arm->get_joint_names();
+
+        int num_points = 3;
+
+        // Create points in the trajectory
+        msg.points.resize(num_points);
+
+        for (int i = 0; i < num_points; ++i)
+        {
+            if (i == 0)
+            {
+                if (phase == arm_action_phase::belt_to_bin)
+                {
+                    msg.points[i].positions = above_conveyor;
+                }
+                else if (phase == arm_action_phase::bin_to_belt)
+                {
+                    msg.points[i].positions = above_bins;
+                }
+                else if (phase == arm_action_phase::item_pickup_adjustment)
+                {
+                    num_points = 1;
+                    msg.points.resize(num_points);
+                    msg.points[i].positions = target;
+                    msg.points[i].time_from_start = ros::Duration(time_per_step);
+                    break;
+                }
+
+                msg.points[i].time_from_start = ros::Duration((i + 1) * time_per_step);
+            }
+            else if (i == 1)
+            {
+                if (phase == arm_action_phase::belt_to_bin)
+                {
+                    msg.points[i].positions = above_bins;
+                }
+                else if (phase == arm_action_phase::bin_to_belt)
+                {
+                    msg.points[i].positions = above_conveyor;
+                }
+
+                msg.points[i].time_from_start = ros::Duration((i + 1) * time_per_step);
+            }
+            else if (i == (num_points - 1))
+            {
+                msg.points[i].positions = target;
+                msg.points[i].time_from_start = ros::Duration((i + 1) * time_per_step);
+            }
+        }
+
+        arm_joint_trajectory_publisher_.publish(msg);
+
+        // Wait to reach target
+        ros::Duration((num_points + 1) * time_per_step).sleep();
+    }
+
+    // Turn gripper on or off
+    bool gripper_control(bool state)
+    {
+        osrf_gear::VacuumGripperControl srv;
+        srv.request.enable = state;
+
+        bool success = gripper_.call(srv);
+
+        if (!success)
+        {
+            ROS_ERROR("Could not enable gripper");
+        }
+
+        return success;
+    }
+
+    // Check if the end effector has reached the target position (within threshold)
+    bool have_reached_target(geometry_msgs::Pose cur, geometry_msgs::Pose target)
+    {
+        // Threshold values
+        float pos_thresh = 0.15; // Meters
+        float rot_thresh = 0.05;
+
+        float pos_err = fabs(cur.position.x - target.position.x) +
+                        fabs(cur.position.y - target.position.y) +
+                        fabs(cur.position.z - target.position.z);
+
+        // ROS_INFO_STREAM("pos_err: " << pos_err << " (" << pos_thresh << ")");
+
+        if (pos_err > pos_thresh)
+        {
+            return false;
+        }
+
+        float qx_err = fabs(fabs(cur.orientation.x) - fabs(target.orientation.x));
+        float qy_err = fabs(fabs(cur.orientation.y) - fabs(target.orientation.y));
+        float qz_err = fabs(fabs(cur.orientation.z) - fabs(target.orientation.z));
+        float qw_err = fabs(fabs(cur.orientation.w) - fabs(target.orientation.w));
+
+        // ROS_INFO_STREAM("qx_err: " << qx_err << " (" << rot_thresh << ")");
+        // ROS_INFO_STREAM("qy_err: " << qy_err << " (" << rot_thresh << ")");
+        // ROS_INFO_STREAM("qz_err: " << qz_err << " (" << rot_thresh << ")");
+        // ROS_INFO_STREAM("qw_err: " << qw_err << " (" << rot_thresh << ")");
+
+        if (qx_err > rot_thresh)
+        {
+            return false;
+        }
+
+        if (qy_err > rot_thresh)
+        {
+            return false;
+        }
+
+        if (qz_err > rot_thresh)
+        {
+            return false;
+        }
+
+        if (qw_err > rot_thresh)
+        {
+            return false;
+        }
+
+        return true;
+    }
+};
+
+int main(int argc, char **argv)
+{
+    ROS_INFO("Starting arm_controller_node");
+
+    // Last argument is the default name of the node.
+    ros::init(argc, argv, "arm_controller_node");
+
+    ros::NodeHandle node;
+
+    ArmRepresentation arm;
+
+    // Set parameters
+    double time_per_step = 3.0; // seconds
+    int retry_attempts = 3;
+    double item_attach_z_adjustment = 0.05; // meters
+    ArmController ac(node, &arm, time_per_step, retry_attempts, item_attach_z_adjustment);
+
+    // Start asynchronous spinner to receive messages on subscribed topics
+    ros::AsyncSpinner spinner(0);
+    spinner.start();
+
+    // Subscribe to ROS topics
+    ros::Subscriber arm_destination_sub = node.subscribe(ARM_DESTINATION_CHANNEL, 1, &ArmController::arm_destination_callback, &ac);
+    ros::Subscriber gripper_state_sub = node.subscribe(GRIPPER_STATE_CHANNEL, 1, &ArmController::gripper_state_callback, &ac);
+    ros::Subscriber joint_state_sub = node.subscribe(ARM_JOINT_STATES_CHANNEL, 1, &ArmController::joint_state_callback, &ac);
+
+    ROS_INFO("Setup complete");
+
+    // Execute callbacks on new data until ctrl-c
+    ros::waitForShutdown();
+
+    return 0;
+}

src/rrrobot_ws/src/rrrobot/src/arm_representation.cpp

@@ -0,0 +1,124 @@
+// arm_representation.cpp
+
+#include "arm_representation.h"
+#include <math.h>
+
+using namespace std;
+
+// Constructor
+ArmRepresentation::ArmRepresentation(const KDL::Frame &base_pose)
+{
+    // Parameters for coordinate transformations of robot links and joints
+    const double base_len = 0.2;
+    const double shoulder_height = 0.1273;
+    const double upper_arm_length = 0.612;
+    const double forearm_length = 0.5723;
+    const double shoulder_offset = 0.220941;
+    const double elbow_offset = -0.1719;
+    const double wrist_1_length = 0.163941 - elbow_offset - shoulder_offset;
+    const double wrist_2_length = 0.1157;
+    const double wrist_3_length = 0.0922;
+
+    // Define chain of links and joints using KDL
+    KDL::Segment linear_arm_actuator("linear_arm_actuator_joint",
+                                     KDL::Joint(KDL::Joint::JointType::None), base_pose);
+    chain.addSegment(linear_arm_actuator);
+
+    const KDL::Vector pos_base(0, 0, base_len / 2);
+    const KDL::Rotation rot_base(KDL::Rotation::Quaternion(0, 0, 0, 1));
+    KDL::Segment base_link("base_link", KDL::Joint(KDL::Joint::JointType::None), //KDL::Joint("linear_arm_actuator_joint", KDL::Joint::JointType::TransY),
+                           KDL::Frame(rot_base, pos_base));
+    chain.addSegment(base_link);
+
+    const KDL::Vector pos_shoulder(0, 0, shoulder_height);
+    const KDL::Rotation rot_shoulder(KDL::Rotation::Quaternion(0, 0, 0, 1));
+    KDL::Segment shoulder_link("shoulder_link", KDL::Joint("linear_arm_actuator_joint", KDL::Joint::JointType::TransY), //KDL::Joint("shoulder_pan_joint", KDL::Joint::JointType::RotZ),
+                               KDL::Frame(rot_shoulder, pos_shoulder));
+    chain.addSegment(shoulder_link);
+
+    const KDL::Vector pos_upper_arm(0, shoulder_offset, 0);
+    const KDL::Rotation rot_upper_arm(KDL::Rotation::Quaternion(0, sqrt(2) / 2, 0, sqrt(2) / 2));
+    KDL::Segment upper_arm_link("upper_arm_link", KDL::Joint("shoulder_pan_joint", KDL::Joint::JointType::RotZ), //KDL::Joint("shoulder_lift_joint", KDL::Joint::JointType::RotY),
+                                KDL::Frame(rot_upper_arm, pos_upper_arm));
+    chain.addSegment(upper_arm_link);
+
+    const KDL::Vector pos_forearm(0, elbow_offset, upper_arm_length);
+    const KDL::Rotation rot_forearm(KDL::Rotation::Quaternion(0, 0, 0, 1));
+    KDL::Segment forearm_link("forearm_link", KDL::Joint("shoulder_lift_joint", KDL::Joint::JointType::RotY), //KDL::Joint("elbow_joint", KDL::Joint::JointType::RotY),
+                              KDL::Frame(rot_forearm, pos_forearm));
+    chain.addSegment(forearm_link);
+
+    const KDL::Vector pos_wrist_1(0, 0, forearm_length);
+    const KDL::Rotation rot_wrist_1(KDL::Rotation::Quaternion(0, sqrt(2) / 2, 0, sqrt(2) / 2));
+    KDL::Segment wrist_1_link("wrist_1_link", KDL::Joint("elbow_joint", KDL::Joint::JointType::RotY), //KDL::Joint("wrist_1_joint", KDL::Joint::JointType::RotY),
+                              KDL::Frame(rot_wrist_1, pos_wrist_1));
+    chain.addSegment(wrist_1_link);
+
+    const KDL::Vector pos_wrist_2(0, wrist_1_length, 0);
+    const KDL::Rotation rot_wrist_2(KDL::Rotation::Quaternion(0, 0, 0, 1));
+    KDL::Segment wrist_2_link("wrist_2_link", KDL::Joint("wrist_1_joint", KDL::Joint::JointType::RotY), //KDL::Joint("wrist_2_joint", KDL::Joint::JointType::RotZ),
+                              KDL::Frame(rot_wrist_2, pos_wrist_2));
+    chain.addSegment(wrist_2_link);
+
+    const KDL::Vector pos_wrist_3(0, 0, wrist_2_length);
+    const KDL::Rotation rot_wrist_3(KDL::Rotation::Quaternion(0, 0, 0, 1));
+    KDL::Segment wrist_3_link("wrist_3_link", KDL::Joint("wrist_2_joint", KDL::Joint::JointType::RotZ), //KDL::Joint("wrist_3_joint", KDL::Joint::JointType::RotY),
+                              KDL::Frame(rot_wrist_3, pos_wrist_3));
+    chain.addSegment(wrist_3_link);
+
+    const KDL::Vector pos_ee(0, wrist_3_length, 0.0);
+    const KDL::Rotation rot_ee(KDL::Rotation::Quaternion(0, 0, sqrt(2) / 2, sqrt(2) / 2));
+    KDL::Segment ee_link("ee_link", KDL::Joint("wrist_3_joint", KDL::Joint::JointType::RotY), //KDL::Joint(KDL::Joint::JointType::None),
+                         KDL::Frame(rot_ee, pos_ee));
+    chain.addSegment(ee_link);
+}
+
+// Use KDL to calculate position of end effector from joint states
+int ArmRepresentation::calculateForwardKinematics(const KDL::JntArray &joint_positions, KDL::Frame &end_effector_pose, int joint_nbr)
+{
+    KDL::ChainFkSolverPos_recursive fk_solver(chain);
+    int status = fk_solver.JntToCart(joint_positions, end_effector_pose, joint_nbr);
+
+    return status;
+}
+
+// Use KDL to calculate joint states from position of end effector
+int ArmRepresentation::calculateInverseKinematics(const vector<double> &cur_configuration,
+                                                  const KDL::Frame &desired_end_effector_pose,
+                                                  KDL::JntArray &final_joint_configuration)
+{
+    KDL::ChainIkSolverPos_LMA ik_solver(chain, 1e-3, 2000, 1e-8);
+    
+    // Convert cur_configuration vector to JntArray
+    int num_joints = chain.getNrOfJoints();
+    KDL::JntArray pos(num_joints);
+    KDL::SetToZero(pos);
+
+    for (int i = 0; i < num_joints; ++i) {
+        pos(i) = cur_configuration[i];
+    }
+
+    int status = ik_solver.CartToJnt(pos, desired_end_effector_pose, final_joint_configuration);
+
+    return status;
+}
+
+// Get vector of joint names
+vector<string> ArmRepresentation::get_joint_names()
+{
+    vector<string> joint_names;
+
+    for (auto it : chain.segments)
+    {
+        if (it.getJoint().getType() != KDL::Joint::JointType::None)
+            joint_names.push_back(it.getJoint().getName());
+    }
+
+    return joint_names;
+}
+
+// Return reference to object
+KDL::Chain *ArmRepresentation::getChain()
+{
+    return &chain;
+}

src/rrrobot_ws/src/rrrobot/src/cv_model.py

@@ -0,0 +1,82 @@
+#!/usr/bin/env python
+# cv_model.py
+
+import rospy
+from std_msgs.msg import String
+
+import os, torch, torchvision
+import numpy as np
+from torch import nn
+from PIL import Image
+
+
+class Model_mobilenet(nn.Module):
+    def __init__(self):
+        super(Model_mobilenet, self).__init__()
+        self.mobilenet = torchvision.models.mobilenet_v2(pretrained=True)
+
+        self.mobilenet.classifier = nn.Sequential(
+            nn.Dropout(p=0.2, inplace=False),
+            nn.Linear(1280, 6, bias=True))
+
+    def forward(self, image):
+        z = self.mobilenet(image)
+        return z
+
+
+def call_back(filename):
+    # prepare input file
+    filename = filename.data
+    input_image = Image.open(filename)
+    if input_image.size[0] < input_image.size[1]:
+            input_image = input_image.transpose(Image.ROTATE_90)
+    input_image = input_image.resize((512, 384))
+    # print(input_image.size)
+    input_image = np.moveaxis(np.asarray(input_image), 2, 0).astype(np.float32)
+    input_tensor = torch.from_numpy(input_image)
+    input_tensor = torch.unsqueeze(input_tensor, dim=0)
+    # print(input_tensor.shape)
+
+    # query model
+    predicted_logits = model(input_tensor).squeeze()
+    # print(predicted_logits.shape)
+    predicted_label = torch.argmax(predicted_logits).detach().numpy().tolist()
+
+    type_dict = {0: 'cardboard',
+                 1: 'glass'    ,
+                 2: 'metal'    ,
+                 3: 'paper'    ,
+                 4: 'plastic'  ,
+                 5: 'trash'    }
+
+    print('type: ', type_dict[predicted_label])
+
+    # publish a message, name of this node is 'cv_model'
+    pub = rospy.Publisher('/cv_model', String, queue_size=10)
+    # rospy.init_node('talker', anonymous=True)
+    rate = rospy.Rate(10) # 10hz
+
+    garbage_type = type_dict[predicted_label]
+    rospy.loginfo(filename + ':' + garbage_type)
+    pub.publish(filename + ':' + garbage_type)
+    # rate.sleep()
+
+
+def listener():
+    # rospy.init_node('listener', anonymous=True)
+    rospy.Subscriber('/current_image', String, call_back)
+    # spin() simply keeps python from exiting until this node is stopped
+    rospy.spin()
+
+
+if __name__ == '__main__':
+    rospy.init_node('cv_node')
+
+    # prepare pretrained model
+    model = Model_mobilenet()
+    model.load_state_dict(torch.load('pytorch_pretrain_model.pt', map_location='cpu'))
+    model = model.to('cpu')
+    model.eval()
+
+    # listen to another node
+    listener()

src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp

@@ -0,0 +1,272 @@
+// depth_camera_node.cpp
+
+#include <algorithm>
+#include <vector>
+#include <string>
+
+#include <ros/ros.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/point_cloud_conversion.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/conversions.h>
+#include <pcl_ros/transforms.h>
+#include <pcl/search/search.h>
+#include <pcl/search/kdtree.h>
+#include <pcl/segmentation/extract_clusters.h>
+
+#include <std_msgs/Float32.h>
+#include <std_msgs/String.h>
+#include <std_srvs/Trigger.h>
+
+#include "topic_names.h"
+
+ros::Publisher pub;
+
+void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr &cloud_msg)
+{
+	// define camera pose
+	float cam_x = 1.0;
+	float cam_y = 1.2;
+	float cam_z = 1.4;
+	// rpy = 0 1.21 0
+
+	// convert from PointCloud to PointCloud2
+	sensor_msgs::PointCloud2 cloud2;
+	sensor_msgs::convertPointCloudToPointCloud2(*cloud_msg, cloud2);
+
+	// transform to world frame
+	sensor_msgs::PointCloud2 worldCloud;
+
+	tf::Transform xform;
+	xform.setOrigin(tf::Vector3(cam_x, cam_y, cam_z));
+	xform.setRotation(tf::Quaternion(0, 0.5697, 0, 0.82185));
+
+	pcl_ros::transformPointCloud("world", xform, cloud2, worldCloud);
+
+	// Container for original & filtered data
+	pcl::PCLPointCloud2 *cloud = new pcl::PCLPointCloud2;
+	pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
+	pcl::PCLPointCloud2 cloud_filtered;
+
+	// Convert to PCL data type
+	pcl_conversions::toPCL(worldCloud, *cloud);
+
+	// Perform the actual filtering
+	pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
+	sor.setInputCloud(cloudPtr);
+	sor.setLeafSize(0.001, 0.001, 0.001);
+	sor.filter(cloud_filtered);
+
+	pcl::PointCloud<pcl::PointXYZ> point_cloud;
+	pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloudPtr(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::fromPCLPointCloud2(cloud_filtered, point_cloud);
+	pcl::copyPointCloud(point_cloud, *point_cloudPtr);
+
+	// Filter out points on surface of belt
+	for (std::size_t i = 0; i < point_cloudPtr->points.size(); i++)
+	{
+		//std::cout << point_cloudPtr->points[i].x << ", " << point_cloudPtr->points[i].y << ", " << point_cloudPtr->points[i].z << std::endl;
+		if (point_cloudPtr->points[i].z < 0.93)
+		{
+			point_cloudPtr->points[i].x = 0;
+			point_cloudPtr->points[i].y = 0;
+			point_cloudPtr->points[i].z = 0;
+		}
+	}
+
+	// Create the KdTree object for the search method of the extraction
+	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
+	tree->setInputCloud(point_cloudPtr);
+
+	std::vector<pcl::PointIndices> cluster_indices;
+	pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
+	ec.setClusterTolerance(0.05); // 5cm
+	ec.setMinClusterSize(10);	  //10
+	ec.setMaxClusterSize(99000000);
+	ec.setSearchMethod(tree);
+	ec.setInputCloud(point_cloudPtr);
+	ec.extract(cluster_indices);
+
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_segmented(new pcl::PointCloud<pcl::PointXYZRGB>);
+
+	int j = 0;
+
+	// store index of object cluster
+	int obj_idx = -1;
+
+	for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
+	{
+		// average all points
+		int s = 0;
+		float x = 0;
+		float y = 0;
+		float z = 0;
+		for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
+		{
+			// convert
+			pcl::PointXYZRGB point;
+			point.x = point_cloudPtr->points[*pit].x;
+			point.y = point_cloudPtr->points[*pit].y;
+			point.z = point_cloudPtr->points[*pit].z;
+
+			// add to running totals
+			s++;
+			x += point.x;
+			y += point.y;
+			z += point.z;
+
+			if (j == 0) //Red	#FF0000	(255,0,0)
+			{
+				point.r = 0;
+				point.g = 0;
+				point.b = 255;
+			}
+			else if (j == 1) //Lime	#00FF00	(0,255,0)
+			{
+				point.r = 0;
+				point.g = 255;
+				point.b = 0;
+			}
+			else if (j == 2) // Blue	#0000FF	(0,0,255)
+			{
+				point.r = 255;
+				point.g = 0;
+				point.b = 0;
+			}
+			else
+			{
+				if (j % 2 == 0)
+				{
+					point.r = 255 * j / (cluster_indices.size());
+					point.g = 128;
+					point.b = 50;
+				}
+				else
+				{
+					point.r = 0;
+					point.g = 255 * j / (cluster_indices.size());
+					point.b = 128;
+				}
+			}
+			point_cloud_segmented->push_back(point);
+		}
+		// calculate center of cluster
+		x /= s;
+		y /= s;
+		z /= s;
+
+		// print cluster center
+		std::cout << "cluster at: " << x << ", " << y << ", " << z << std::endl;
+
+		// check if center is within box
+		if (x < 1.25 && x > 1.15 && y < cam_y + .05 && y > cam_y - .05)
+		{
+			// object detected
+			std::cout << "object in position" << std::endl;
+			obj_idx = j;
+		}
+
+		j++;
+	}
+
+	// identify surfaces of object cluster
+	if (obj_idx != -1)
+	{
+		// create new cloud containing only object cluster
+		pcl::PointCloud<pcl::PointXYZ> obj_cloud;
+		std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin() + obj_idx;
+		for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
+		{
+			pcl::PointXYZ point;
+			point.x = point_cloudPtr->points[*pit].x;
+			point.y = point_cloudPtr->points[*pit].y;
+			point.z = point_cloudPtr->points[*pit].z;
+			obj_cloud.push_back(point);
+		}
+
+		std::cout << "object cluster size: " << obj_cloud.points.size() << std::endl;
+
+		// create 3D bounding box.
+		float xmin = 10.0;
+		float ymin = 10.0;
+		float zmin = 0.92;
+		float xmax = 0;
+		float ymax = 0;
+		float zmax = 0;
+
+		for (int i = 0; i < obj_cloud.points.size(); ++i)
+		{
+			xmin = xmin < obj_cloud.points[i].x ? xmin : obj_cloud.points[i].x;
+			ymin = ymin < obj_cloud.points[i].y ? ymin : obj_cloud.points[i].y;
+			//zmin = zmin < obj_cloud.points[i].z ? zmin : obj_cloud.points[i].z;
+			xmax = xmax > obj_cloud.points[i].x ? xmax : obj_cloud.points[i].x;
+			ymax = ymax > obj_cloud.points[i].y ? ymax : obj_cloud.points[i].y;
+			zmax = zmax > obj_cloud.points[i].z ? zmax : obj_cloud.points[i].z;
+		}
+
+		// determine target pose
+		float area_top = (ymax - ymin) * (xmax - xmin);
+		float area_side = (zmax - zmin) * (ymax - ymin);
+
+		std::cout << "object top area: " << area_top << std::endl;
+		std::cout << "object side area: " << area_side << std::endl;
+
+		geometry_msgs::Pose pose;
+		geometry_msgs::Point p;
+		geometry_msgs::Quaternion q;
+
+		// pick up from top (-z direction)
+		if (area_top > .002)
+		{
+			std::cout << "picking up object from top" << std::endl;
+			p.x = (xmin + xmax) / 2;
+			p.y = (ymin + ymax) / 2;
+			p.z = zmax;
+
+			// rpy = 0 0 0
+			q.x = 0;
+			q.y = 0.707;
+			q.z = 0;
+			q.w = 0.707;
+		}
+		else
+		{
+			// pick up from front (+x direction)
+			std::cout << "picking up object from front" << std::endl;
+			p.x = xmin;
+			p.y = (ymin + ymax) / 2;
+			p.z = (zmax + zmin) / 2;
+
+			// rpy = -pi/2 0 pi/2
+			q.x = 0.707;
+			q.y = 0.0;
+			q.z = 0.0;
+			q.w = 0.707;
+		}
+
+		pose.position = p;
+		pose.orientation = q;
+
+		// publish pose
+		pub.publish(pose);
+	}
+}
+
+int main(int argc, char **argv)
+{
+	// Last argument is the default name of the node.
+	ros::init(argc, argv, "depth_camera_node");
+
+	ros::NodeHandle node;
+
+	// Subscribe to depth camera topic
+	ros::Subscriber sub = node.subscribe("/ariac/depth_camera_1", 1, depth_camera_callback);
+
+	// Publish object's current location to topic for RRRobot node to listen to
+	pub = node.advertise<geometry_msgs::Pose>(DESIRED_GRASP_POSE_CHANNEL, 1);
+
+	ros::spin();
+}

src/rrrobot_ws/src/rrrobot/src/image_display.py

@@ -0,0 +1,23 @@
+#!/usr/bin/env python
+# image_display.py
+
+import rospy
+from std_msgs.msg import String
+import matplotlib.pyplot as plt
+import matplotlib.image as mpimg
+
+def callback(data):
+    print("IMAGE DISPLAY CALLBACK")
+    file = data.data
+
+    img = mpimg.imread(file)
+    imgplot = plt.imshow(img)
+    plt.show()
+
+def listener():
+    rospy.init_node('image_display', anonymous=False)
+    rospy.Subscriber("/current_image", String, callback)
+    rospy.spin()
+
+if __name__ == '__main__':
+    listener()

src/rrrobot_ws/src/rrrobot/src/model_insertion_plugin.cpp

@@ -0,0 +1,76 @@
+// model_insertion_plugin.cpp
+
+#include <ignition/math/Pose3.hh>
+#include "gazebo/physics/physics.hh"
+#include "gazebo/common/common.hh"
+#include "gazebo/gazebo.hh"
+
+#include "ros/ros.h"
+#include <std_msgs/String.h>
+#include "rrrobot/model_insertion.h"
+
+#include <string>
+#include <memory>
+#include <iostream>
+
+namespace gazebo
+{
+class ModelInsertion : public WorldPlugin
+{
+public:
+    void Load(physics::WorldPtr _parent, sdf::ElementPtr /*_sdf*/)
+    {
+        std::cout << "Loading model insertion plugin" << std::endl;
+
+        parent = _parent;
+
+        if (!ros::isInitialized())
+        {
+            int argc = 0;
+            char **argv = NULL;
+            ros::init(argc, argv, "arm");
+        }
+
+        nh.reset(new ros::NodeHandle());
+        subscriber = nh->subscribe("object_to_load", 1000, &ModelInsertion::insertModel, this);
+    }
+
+    void insertModel(const rrrobot::model_insertion &msg)
+    {
+        std::cout << "Received message to load model: " << msg.model_name << std::endl;
+
+        // Create a new transport node
+        transport::NodePtr node(new transport::Node());
+
+        // Initialize the node with the world name
+        node->Init(parent->Name());
+
+        // Create a publisher on the ~/factory topic
+        transport::PublisherPtr factoryPub =
+            node->Advertise<msgs::Factory>("~/factory");
+
+        // Create the message
+        msgs::Factory to_pub;
+
+        // Model file to load
+        to_pub.set_sdf_filename(std::string("model://") + msg.model_name);
+
+        // Pose to initialize the model to
+        msgs::Set(to_pub.mutable_pose(),
+                  ignition::math::Pose3d(
+                      ignition::math::Vector3d(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z),
+                      ignition::math::Quaterniond(msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w)));
+
+        // Send the message
+        factoryPub->Publish(to_pub);
+    }
+
+private:
+    physics::WorldPtr parent;
+    std::unique_ptr<ros::NodeHandle> nh;
+    ros::Subscriber subscriber;
+};
+
+// Register this plugin with the simulator
+GZ_REGISTER_WORLD_PLUGIN(ModelInsertion)
+} // namespace gazebo

src/rrrobot_ws/src/rrrobot/src/object_spawner_node.cpp

@@ -0,0 +1,127 @@
+// object_spawner_node.cpp
+
+#include <vector>
+#include <string>
+#include <stdlib.h>
+#include <time.h>
+#include <fstream>
+
+#include <ros/ros.h>
+
+#include <osrf_gear/VacuumGripperState.h>
+#include <geometry_msgs/Point.h>
+#include <std_msgs/String.h>
+#include "rrrobot/model_insertion.h"
+#include <kdl/frames.hpp>
+
+#include <iostream>
+using namespace std;
+
+class ObjectSpawner
+{
+public:
+    ObjectSpawner(const std::string &model_file = "/app/rrrobot_ws/src/gazebo_models/model_mappings.txt")
+        : DEFAULT_SPAWN_POINT(1.21825, 4.0, 0.937978),
+          CONVEYOR_WIDTH(0.391404)
+    {
+        // DEFAULT_SPAWN_POINT.x = 0.2516105;
+        // DEFAULT_SPAWN_POINT.y = 5.474367;
+        // DEFAULT_SPAWN_POINT.z = 0.935669;
+        model_pub = nh.advertise<rrrobot::model_insertion>("/object_to_load", 1000);
+        image_pub = nh.advertise<std_msgs::String>("/current_image", 1000);
+        sub = nh.subscribe("/ariac/arm1/gripper/state", 1000, &ObjectSpawner::objectPlacedCallback, this);
+        srand(time(NULL));
+        read_in_items(model_file);
+    }
+
+    void objectPlacedCallback(const osrf_gear::VacuumGripperState &state)
+    {
+        static bool prev_state = false;
+        static bool placed = false;
+
+        if (!placed)
+        {
+            spawn_item();
+            placed = true;
+        }
+
+        //cout << "State received: " << (state.enabled ? "true" : "false") << endl;
+        // gripper is released
+        if (!state.enabled && prev_state)
+            spawn_item();
+
+        prev_state = state.enabled;
+    }
+
+private:
+    struct ObjectMapping
+    {
+        std::string model_name;
+        std::string image_file;
+        std::string correct_classification;
+    };
+
+    const KDL::Vector DEFAULT_SPAWN_POINT;
+    const float CONVEYOR_WIDTH;
+    ros::NodeHandle nh;
+    ros::Publisher model_pub;
+    ros::Publisher image_pub;
+    ros::Subscriber sub;
+    std::vector<ObjectMapping> models; // TODO: just string? or do we need
+                                       //to store the correct classification too?
+
+    void read_in_items(const std::string &model_file)
+    {
+        std::ifstream file(model_file);
+        std::string cur;
+        while (std::getline(file, cur))
+        {
+            // cout << "cur: " << cur << " cur.find(,): " << cur.find(",") << endl;
+            // line format: model_name, image_file, model_classification
+            ObjectMapping cur_model;
+            cur_model.model_name = cur.substr(0, cur.find(","));
+            cur.erase(0, cur.find(",") + 1);
+            cur_model.image_file = cur.substr(0, cur.find(","));
+            cur.erase(0, cur.find(",") + 1);
+            cur_model.correct_classification = cur;
+            models.push_back(cur_model);
+        }
+    }
+
+    void spawn_item()
+    {
+        if (ros::ok())
+        {
+            rrrobot::model_insertion msg;
+            std_msgs::String image_file_msg;
+
+            int rand_idx = rand() % models.size();
+            msg.model_name = models[rand_idx].model_name;
+            image_file_msg.data = models[rand_idx].image_file;
+            cout << "Spawning " << msg.model_name << endl;
+
+            // Determine position to spawn items
+            // can place objects on conveyor belt just upstream from the arms
+            // at roughly (0.2516105, 5.474367, 0.935669)
+            // x range: 1.022548-1.413952
+            // y range: anything (essentially)
+            // z
+            msg.pose.position.x = DEFAULT_SPAWN_POINT.x() /* + some random error */;
+            msg.pose.position.y = DEFAULT_SPAWN_POINT.y();
+            msg.pose.position.z = DEFAULT_SPAWN_POINT.z() + 1.0;
+
+            model_pub.publish(msg);
+            image_pub.publish(image_file_msg);
+            ros::spinOnce();
+        }
+    }
+};
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "object_spawner");
+
+    ObjectSpawner spawner;
+
+    ros::spin();
+}

src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp

@@ -0,0 +1,227 @@
+// rrrobot_node.cpp
+
+#include <algorithm>
+#include <vector>
+#include <string>
+#include <stdlib.h>
+#include <time.h>
+#include <fstream>
+#include <iostream>
+
+#include <ros/ros.h>
+
+#include <osrf_gear/LogicalCameraImage.h>
+#include <osrf_gear/Order.h>
+#include <osrf_gear/Proximity.h>
+#include <osrf_gear/VacuumGripperState.h>
+#include <osrf_gear/ConveyorBeltControl.h>
+#include <sensor_msgs/JointState.h>
+#include <sensor_msgs/LaserScan.h>
+#include <sensor_msgs/Range.h>
+#include <std_msgs/Float32.h>
+#include <std_msgs/String.h>
+#include <std_srvs/Trigger.h>
+#include <trajectory_msgs/JointTrajectory.h>
+
+#include "topic_names.h"
+#include "rrrobot/arm_command.h"
+
+using namespace std;
+
+class Position
+{
+public:
+	float x, y, z;
+
+	Position(float x, float y, float z) : x(x), y(y), z(z){};
+	Position() : x(0), y(0), z(0) {}
+};
+
+class RRRobot
+{
+public:
+	RRRobot(ros::NodeHandle &node)
+		: current_robot_state(RobotState::WAITING_FOR_CLASSIFICATION | RobotState::WAITING_FOR_GRAB_LOCATION),
+		  nh(node)
+	{
+		cv_classification_sub = nh.subscribe(CV_CLASSIFICATION_CHANNEL, 1000, &RRRobot::cv_classification_callback, this);
+		gripper_state_sub = nh.subscribe(GRIPPER_STATE_CHANNEL, 1000, &RRRobot::gripper_state_callback, this);
+		depth_camera_sub = nh.subscribe(DESIRED_GRASP_POSE_CHANNEL, 1000, &RRRobot::grasp_pose_callback, this);
+
+		conveyor_pub = nh.serviceClient<osrf_gear::ConveyorBeltControl>(CONVEYOR_POWER_CHANNEL);
+		arm_destination_pub = nh.advertise<rrrobot::arm_command>(ARM_DESTINATION_CHANNEL, 1000);
+
+		// start competition
+		ros::ServiceClient comp_start = nh.serviceClient<std_srvs::Trigger>(START_COMPETITION_CHANNEL);
+		std_srvs::Trigger trg;
+		comp_start.call(trg);
+	}
+
+	void cv_classification_callback(const std_msgs::String &classification)
+	{
+		std::string type = classification.data.substr(classification.data.find(":") + 1);
+
+		Position drop_point = destination(type);
+		desired_drop_pose.position.x = drop_point.x;
+		desired_drop_pose.position.y = drop_point.y;
+		desired_drop_pose.position.z = drop_point.z;
+		
+		// update state
+		if (current_robot_state & RobotState::WAITING_FOR_GRAB_LOCATION)
+		{
+			current_robot_state &= ~RobotState::WAITING_FOR_CLASSIFICATION;
+		}
+		else if ((current_robot_state & RobotState::MOVING_ARM) == 0x0)
+		{
+			current_robot_state = RobotState::MOVING_ARM;
+
+			// tell the arm to move to grab the object
+			publish_arm_command();
+		}
+
+		// print_state();
+	}
+
+	void gripper_state_callback(const osrf_gear::VacuumGripperState &state)
+	{
+		if (state.attached)
+		{
+			current_robot_state = RobotState::MOVING_ARM;
+
+			if (!gripper_state.attached)
+			{
+				// start the conveyor belt again
+				set_conveyor(100);
+			}
+		}
+		// just dropped the object
+		else if (gripper_state.attached /* && !state.attached */)
+		{
+			current_robot_state = RobotState::WAITING_FOR_CLASSIFICATION | RobotState::WAITING_FOR_GRAB_LOCATION;
+		}
+
+		// store current state
+		gripper_state = state;
+
+		// print_state();
+	}
+
+	void grasp_pose_callback(const geometry_msgs::Pose &grasp_pose)
+	{
+		// stop conveyor belt
+		set_conveyor(0);
+
+		desired_grasp_pose = grasp_pose;
+		// Add z offset so end effector doesn't hit object
+		desired_grasp_pose.position.z += 0.01;
+
+		if (current_robot_state & RobotState::WAITING_FOR_CLASSIFICATION)
+		{
+			current_robot_state &= ~RobotState::WAITING_FOR_GRAB_LOCATION;
+		}
+		else if ((current_robot_state & RobotState::MOVING_ARM) == 0x0)
+		{
+			current_robot_state = RobotState::MOVING_ARM;
+
+			publish_arm_command();
+		}
+
+		// print_state();
+	}
+
+private:
+	enum RobotState
+	{
+		WAITING_FOR_CLASSIFICATION = 0x1,
+		WAITING_FOR_GRAB_LOCATION = 0x1 << 1,
+		MOVING_ARM = 0x1 << 2
+	};
+
+	int current_robot_state;
+	osrf_gear::VacuumGripperState gripper_state;
+	geometry_msgs::Pose desired_grasp_pose;
+	geometry_msgs::Pose desired_drop_pose;
+
+	ros::NodeHandle nh;
+
+	ros::Subscriber cv_classification_sub;
+	ros::Subscriber gripper_state_sub; // know when item has been grabbed, so conveyor can be started
+	ros::Subscriber depth_camera_sub;  // get desired grab location
+
+	ros::ServiceClient conveyor_pub;
+	ros::Publisher arm_destination_pub;
+
+	Position trash_bin = Position(-0.3, 0.383, 1);
+	Position recycle_bin = Position(-0.3, 1.15, 1);
+
+	// Determine item destination bin based on classification
+	Position destination(const std::string &type) const
+	{
+		Position pos;
+
+		if (type == "trash")
+		{
+			pos = trash_bin;
+		}
+		else
+		{
+			pos = recycle_bin;
+		}
+
+		return pos;
+	}
+
+	// Publish message including grab and drop off location for arm controller
+	void publish_arm_command()
+	{
+		rrrobot::arm_command cmd;
+		cmd.grab_location = desired_grasp_pose;
+		cmd.drop_location = desired_drop_pose;
+
+		arm_destination_pub.publish(cmd);
+		ros::spinOnce();
+	}
+
+	// Set conveyor power (0 or 50-100)
+	void set_conveyor(int power)
+	{
+		if (power != 0 && (power < 50 || power > 100))
+		{
+			return;
+		}
+
+		osrf_gear::ConveyorBeltControl cmd;
+		cmd.request.power = power;
+		conveyor_pub.call(cmd);
+	}
+
+	// Print current state for debugging
+	void print_state()
+	{
+		if (current_robot_state & RobotState::WAITING_FOR_CLASSIFICATION)
+		{
+			cout << "Waiting for classification\t";
+		}
+		if (current_robot_state & RobotState::WAITING_FOR_GRAB_LOCATION)
+		{
+			cout << "Waiting for grab location\t";
+		}
+		if (current_robot_state & RobotState::MOVING_ARM)
+		{
+			cout << "Moving Arm\t";
+		}
+		cout << endl;
+	}
+};
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "rrrobot");
+
+	ros::NodeHandle node;
+
+	RRRobot robot(node);
+	ros::spin(); // This executes callbacks on new data until ctrl-c.
+
+	return 0;
+}

src/rrrobot_ws/src/rrrobot/test/test_arm.cpp

@@ -0,0 +1,109 @@
+// test_arm.cpp
+
+#include <algorithm>
+#include <vector>
+#include <string>
+#include <memory>
+
+#include <ros/ros.h>
+
+#include <std_msgs/Float32.h>
+#include <std_msgs/String.h>
+#include <std_srvs/Trigger.h>
+
+#include <osrf_gear/VacuumGripperControl.h>
+#include <trajectory_msgs/JointTrajectory.h>
+
+#include "topic_names.h"
+#include "rrrobot/arm_command.h"
+
+#include <kdl/chain.hpp>
+#include <kdl/segment.hpp>
+#include <kdl/rigidbodyinertia.hpp>
+#include <kdl/rotationalinertia.hpp>
+#include <kdl/joint.hpp>
+#include <kdl/frames.hpp>
+#include <LinearMath/btTransform.h>
+#include <kdl/chainfksolver.hpp>
+#include <kdl/chainiksolverpos_lma.hpp>
+#include <kdl/chainfksolverpos_recursive.hpp>
+#include <kdl/chainidsolver_recursive_newton_euler.hpp>
+#include <kdl/frames_io.hpp>
+
+#include <fstream>
+#include <iostream>
+#include <string>
+
+#include "arm_representation.h"
+
+using namespace std;
+
+
+int main(int argc, char **argv)
+{
+    ArmRepresentation arm;
+
+    KDL::JntArray pos(arm.getChain()->getNrOfJoints());
+    KDL::SetToZero(pos);
+
+    int joint;
+
+    // std::cout << "Enter joint to exercise: ";
+    // std::cin >> joint;
+
+    KDL::Frame end_effector_pose;
+    std::ofstream f("arm_test.debug");
+    int error_code;
+    for (joint = 0; joint < arm.getChain()->getNrOfJoints(); ++joint)
+    {
+        for (double pos_val = 0.0; pos_val <= 2 * M_PI; pos_val += 0.1)
+        {
+            pos(joint) = pos_val;
+            error_code = arm.calculateForwardKinematics(pos, end_effector_pose);
+
+            if (error_code != 0)
+            {
+                ROS_ERROR("Forward Kinematics Failure: %i", error_code);
+            }
+
+            f << end_effector_pose.p.x() << "," << end_effector_pose.p.y() << "," << end_effector_pose.p.z() << '\n';
+
+            error_code = arm.calculateInverseKinematics(vector<double>(arm.getChain()->getNrOfJoints(), 0.0), end_effector_pose, pos);
+
+            if (error_code != 0)
+            {
+                ROS_ERROR("Inverse Kinematics Failure: %i", error_code);
+            }
+        }
+    }
+
+    double linear_arm_actuator_joint, shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint;
+
+    while (true)
+    {
+        cout << "linear_arm_actuator_joint: ";
+        cin >> pos(0); //linear_arm_actuator_joint;
+        cout << "shoulder_pan_joint: ";
+        cin >> pos(1); //shoulder_pan_joint;
+        cout << "shoulder_lift_joint: ";
+        cin >> pos(2); //shoulder_lift_joint;
+        cout << "elbow_joint: ";
+        cin >> pos(3); //elbow_joint;
+        cout << "wrist_1_joint: ";
+        cin >> pos(4); //wrist_1_joint;
+        cout << "wrist_2_joint: ";
+        cin >> pos(5); //wrist_2_joint;
+        cout << "wrist_3_joint: ";
+        cin >> pos(6); //wrist_3_joint;
+
+        error_code = arm.calculateForwardKinematics(pos, end_effector_pose);
+
+        if (error_code != 0)
+        {
+            ROS_ERROR("Forward kinematics failure: %i", error_code);
+        }
+
+        cout << "x,y,z: " << end_effector_pose.p.x() << ", " << end_effector_pose.p.y() << ", " << end_effector_pose.p.z() << endl;
+        cout << end_effector_pose.M << endl;
+    }
+}

src/rrrobot_ws/src/rrrobot/test/test_insert_object.cpp

@@ -0,0 +1,41 @@
+// test_insert_object.cpp
+
+#include "rrrobot/model_insertion.h"
+#include "ros/ros.h"
+
+#include <iostream>
+
+using std::cin;
+using std::cout;
+using std::endl;
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "insert_models");
+
+    ros::NodeHandle nh;
+
+    ros::Publisher pub = nh.advertise<rrrobot::model_insertion>("/object_to_load", 1000);
+
+    while (ros::ok())
+    {
+        rrrobot::model_insertion msg;
+        cout << "Enter the model name (or 'q' to quit): ";
+        cin >> msg.model_name;
+
+        if (msg.model_name == "q") {
+            return 0;
+        }
+
+        cout << "Enter the pose: " << endl;
+        cout << "x: ";
+        cin >> msg.pose.position.x;
+        cout << "y: ";
+        cin >> msg.pose.position.y;
+        cout << "z: ";
+        cin >> msg.pose.position.z;
+
+        pub.publish(msg);
+        ros::spinOnce();
+    }
+}

src/rrrobot_ws/world/gear.py

@@ -0,0 +1,675 @@
+#!/usr/bin/env python
+
+# Software License Agreement (Apache License)
+#
+# Copyright 2016 Open Source Robotics Foundation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from __future__ import print_function
+
+import argparse
+import math
+import os
+import random
+import subprocess
+import sys
+
+import em
+import rospkg
+import yaml
+
+rospack = rospkg.RosPack()
+#world_dir = '/home/rrrobot/rrrobot_ws/world' 
+world_dir = os.path.join(rospack.get_path('osrf_gear'), 'worlds')
+launch_dir = os.path.join(rospack.get_path('osrf_gear'), 'launch')
+template_files = [
+    #os.path.join(world_dir, 'gear.world.template'),
+    '/app/rrrobot_ws/world/gear.world.template',
+    os.path.join(launch_dir, 'gear.launch.template'),
+    os.path.join(launch_dir, 'gear.urdf.xacro.template'),
+]
+arm_template_file = os.path.join(launch_dir, 'arm.urdf.xacro.template')
+arm_configs = {
+    'arm1': {
+        'arm_type': 'ur10',
+        'pose': {
+            'xyz': [0.3, 0.92, 0.9],
+            'rpy': [0.0, 0.0, 0.0]
+        },
+        'default_initial_joint_states': {
+            'elbow_joint': 2.14,
+            'linear_arm_actuator_joint': 0,
+            'shoulder_lift_joint': -2.0,
+            'shoulder_pan_joint': 3.14,
+            'wrist_1_joint': 3.27,
+            'wrist_2_joint': -1.51,
+            'wrist_3_joint': 0,
+        }
+    },
+    # 'arm2': {
+    #     'arm_type': 'ur10',
+    #     'pose': {
+    #         'xyz': [0.3, -0.92, 0.9],
+    #         'rpy': [0.0, 0.0, 0.0]
+    #     },
+    #     'default_initial_joint_states': {
+    #         'elbow_joint': 2.14,
+    #         'linear_arm_actuator_joint': 0,
+    #         'shoulder_lift_joint': -2.0,
+    #         'shoulder_pan_joint': 3.14,
+    #         'wrist_1_joint': 3.27,
+    #         'wrist_2_joint': -1.51,
+    #         'wrist_3_joint': 0,
+    #     }
+    # },
+}
+possible_products = [
+    'disk_part',
+    'gasket_part',
+    'gear_part',
+    'piston_rod_part',
+    'pulley_part',
+]
+sensor_configs = {
+    'break_beam': None,
+    'camera': None,
+    'proximity_sensor': None,
+    'logical_camera': None,
+    'laser_profiler': None,
+    'depth_camera': None,
+    'quality_control': None,
+}
+default_sensors = {
+    # 'quality_control_sensor_1': {
+    #     'type': 'quality_control',
+    #     'pose': {
+    #         'xyz': [0.3, 3.5, 1.5],
+    #         'rpy': [0, 1.574, -1.574]
+    #     }
+    # },
+    # 'quality_control_sensor_2': {
+    #     'type': 'quality_control',
+    #     'pose': {
+    #         'xyz': [0.3, -3.5, 1.5],
+    #         'rpy': [0, 1.574, 1.574]
+    #     }
+    # },
+}
+default_belt_models = {
+}
+bin_width = 0.6
+bin_depth = 0.6
+bin_height = 0.72
+bin_angle = 0.0
+default_bin_origins = {
+    # 'bin1': [-0.3, -1.916, 0],
+    # 'bin2': [-0.3, -1.15, 0],
+    # 'bin3': [-0.3, -0.383, 0],
+    'bin4': [-0.3, 0.383, 0],
+    'bin5': [-0.3, 1.15, 0],
+    # 'bin6': [-0.3, 1.916, 0],
+}
+
+configurable_options = {
+    'insert_models_over_bins': False,
+    'disable_shadows': False,
+    'belt_population_cycles': 5,
+    'gazebo_state_logging': False,
+    'spawn_extra_models': False,
+    'unthrottled_physics_update': False,
+    'model_type_aliases': {
+        'belt_model_type1': 'part1',
+        'belt_model_type2': 'part2',
+    },
+    'visualize_sensor_views': False,
+    'visualize_drop_regions': False,
+}
+default_time_limit = 600  # seconds
+max_count_per_model = 30  # limit on the number of instances of each model type
+
+def initialize_model_id_mappings(random_seed=None):
+    global global_model_count, model_id_mappings
+    global_model_count = {}  # global count of how many times a model type has been created
+
+    randomize = False
+    if random_seed is not None:
+        randomize = True
+        random.seed(random_seed)
+
+    # Initialize the mapping between model index and ID that will exist for each model type
+    model_id_mappings = {}
+
+    # Initialize the list of random IDs that will be used in the mappings
+    # The IDs will be unique across different model types
+    max_model_id = max_count_per_model * len(possible_products)  # can be larger for more spread
+    random_ids = random.sample(range(0, max_model_id), max_model_id)
+    for model_type in possible_products:
+        if not randomize:
+            # Just use ordinary mapping
+            model_id_mappings[model_type] = list(range(1, max_count_per_model + 1))
+        else:
+            # Use random IDs for the mapping
+            model_id_mappings[model_type] = random_ids[:max_count_per_model]
+            del random_ids[:max_count_per_model]
+
+
+
+# Helper for converting strings to booleans; copied from https://stackoverflow.com/a/43357954
+def str2bool(v):
+    if v.lower() in ('yes', 'true', 't', 'y', '1'):
+        return True
+    elif v.lower() in ('no', 'false', 'f', 'n', '0'):
+        return False
+    else:
+        raise argparse.ArgumentTypeError('Boolean value expected.')
+
+
+def prepare_arguments(parser):
+    add = parser.add_argument
+    add('-n', '--dry-run', action='store_true', default=False,
+        help='print generated files to stdout, but do not write them to disk')
+    add('-v', '--verbose', action='store_true', default=False,
+        help='output additional logging to console')
+    add('-o', '--output', default='/tmp/ariac/',
+        help='directory in which to output the generated files')
+    add('--development-mode', '-d', action='store_true', default=False,
+        help='if true the competition mode environment variable will not be set (default false)')
+    add('--no-gui', action='store_true', default=False,
+        help="don't run the gazebo client gui")
+    add('-l', '--state-logging', action='store', type=str2bool, nargs='?',
+        help='generate gazebo state logs (will override config file option)')
+    add('--log-to-file', action='store_true', default=False,
+        help='direct the output of the gazebo ros node to log file instead of the console')
+    add('--visualize-sensor-views', action='store_true', default=False,
+        help='visualize the views of sensors in gazebo')
+    mex_group = parser.add_mutually_exclusive_group(required=False)
+    add = mex_group.add_argument
+    add('config', nargs='?', metavar='CONFIG',
+        help='yaml string that is the configuration')
+    add('-f', '--file', nargs='+', help='list of paths to yaml files that contain the '
+        'configuration (contents will be concatenated)')
+
+
+eval_local_vars = {n: getattr(math, n) for n in dir(math) if not n.startswith('__')}
+
+
+def expand_to_float(val):
+    if isinstance(val, str):
+        return float(eval(val, {}, eval_local_vars))
+    return float(val)
+
+
+def expand_yaml_substitutions(yaml_dict):
+    for k, v in yaml_dict.items():
+        if isinstance(v, dict):
+            yaml_dict[k] = expand_yaml_substitutions(v)
+        if k in ['xyz', 'rpy']:
+            yaml_dict[k] = [expand_to_float(x) for x in v]
+        if k in ['initial_joint_states']:
+            yaml_dict[k] = {kp: expand_to_float(vp) for kp, vp in v.items()}
+    return yaml_dict
+
+
+class ArmInfo:
+    def __init__(self, name, arm_type, initial_joint_states, pose):
+        self.name = name
+        self.type = arm_type
+        self.initial_joint_states = initial_joint_states
+        self.pose = pose
+
+
+class ModelInfo:
+    def __init__(self, model_type, pose, reference_frame):
+        self.type = model_type
+        self.pose = pose
+        self.reference_frame = reference_frame
+
+
+class SensorInfo:
+    def __init__(self, name, sensor_type, pose):
+        self.name = name
+        self.type = sensor_type
+        self.pose = pose
+
+
+class PoseInfo:
+    def __init__(self, xyz, rpy):
+        self.xyz = [str(f) for f in xyz]
+        self.rpy = [str(f) for f in rpy]
+
+
+class DropRegionInfo:
+    def __init__(self, name, drop_region_min, drop_region_max, destination, frame, model_type):
+        self.name = name
+        self.min = [str(f) for f in drop_region_min]
+        self.max = [str(f) for f in drop_region_max]
+        self.destination = destination
+        self.frame = frame
+        self.type = model_type
+
+
+def get_field_with_default(data_dict, entry, default_value):
+    if entry in data_dict:
+        return data_dict[entry]
+    else:
+        return default_value
+
+
+def get_required_field(entry_name, data_dict, required_entry):
+    if required_entry not in data_dict:
+        print("Error: '{0}' entry does not contain a required '{1}' entry"
+              .format(entry_name, required_entry),
+              file=sys.stderr)
+        sys.exit(1)
+    return data_dict[required_entry]
+
+
+def replace_type_aliases(model_type):
+    if model_type in configurable_options['model_type_aliases']:
+        model_type = configurable_options['model_type_aliases'][model_type]
+    return model_type
+
+
+def model_count_post_increment(model_type):
+    global global_model_count
+    try:
+        count = global_model_count[model_type]
+    except KeyError:
+        count = 0
+    global_model_count[model_type] = count + 1
+    return count
+
+
+def get_next_model_id(model_type):
+    return model_id_mappings[model_type][model_count_post_increment(model_type)]
+
+
+def create_pose_info(pose_dict, offset=None):
+    xyz = get_field_with_default(pose_dict, 'xyz', [0, 0, 0])
+    rpy = get_field_with_default(pose_dict, 'rpy', [0, 0, 0])
+    for key in pose_dict:
+        if key not in ['xyz', 'rpy']:
+            print("Warning: ignoring unknown entry in 'pose': " + key, file=sys.stderr)
+    if offset is not None:
+        xyz = [sum(i) for i in zip(xyz, offset)]
+    return PoseInfo(xyz, rpy)
+
+
+def create_arm_info(name, arm_dict):
+    arm_type = arm_dict['arm_type']
+    initial_joint_states = arm_dict['default_initial_joint_states']
+    pose = create_pose_info(arm_dict['pose'])
+    return ArmInfo(name, arm_type, initial_joint_states, pose)
+
+
+def create_sensor_info(name, sensor_data, allow_protected_sensors=False, offset=None):
+    sensor_type = get_required_field(name, sensor_data, 'type')
+    pose_dict = get_required_field(name, sensor_data, 'pose')
+    for key in sensor_data:
+        if key not in ['type', 'pose']:
+            print("Warning: ignoring unknown entry in '{0}': {1}"
+                  .format(name, key), file=sys.stderr)
+    if sensor_type not in sensor_configs:
+        if not allow_protected_sensors:
+            print("Error: given sensor type '{0}' is not one of the known sensor types: {1}"
+                  .format(sensor_type, sensor_configs.keys()), file=sys.stderr)
+            sys.exit(1)
+    pose_info = create_pose_info(pose_dict, offset=offset)
+    return SensorInfo(name, sensor_type, pose_info)
+
+
+def create_sensor_infos(sensors_dict, allow_protected_sensors=False, offset=None):
+    sensor_infos = {}
+    for name, sensor_data in sensors_dict.items():
+        sensor_infos[name] = create_sensor_info(
+            name, sensor_data,
+            allow_protected_sensors=allow_protected_sensors, offset=offset)
+    return sensor_infos
+
+
+def create_model_info(model_name, model_data):
+    model_type = get_required_field(model_name, model_data, 'type')
+    model_type = replace_type_aliases(model_type)
+    pose_dict = get_required_field(model_name, model_data, 'pose')
+    reference_frame = get_field_with_default(model_data, 'reference_frame', '')
+    for key in model_data:
+        if key not in ['type', 'pose', 'reference_frame']:
+            print("Warning: ignoring unknown entry in '{0}': {1}"
+                  .format(model_name, key), file=sys.stderr)
+    pose_info = create_pose_info(pose_dict)
+    return ModelInfo(model_type, pose_info, reference_frame)
+
+
+def create_models_to_spawn_infos(models_to_spawn_dict):
+    models_to_spawn_infos = {}
+    for reference_frame, reference_frame_data in models_to_spawn_dict.items():
+        models = get_required_field(reference_frame, reference_frame_data, 'models')
+        for model_name, model_to_spawn_data in models.items():
+            model_to_spawn_data['reference_frame'] = reference_frame
+            model_info = create_model_info(model_name, model_to_spawn_data)
+            # assign each model a unique name because gazebo can't do this
+            # if the models all spawn at the same time
+            scoped_model_name = reference_frame.replace('::', '|') + '|' + \
+                model_info.type + '_' + str(get_next_model_id(model_info.type))
+            models_to_spawn_infos[scoped_model_name] = model_info
+    return models_to_spawn_infos
+
+
+def create_models_over_bins_infos(models_over_bins_dict):
+    models_to_spawn_infos = {}
+    for bin_name, bin_dict in models_over_bins_dict.items():
+        if bin_name in default_bin_origins:
+            offset_xyz = [
+                default_bin_origins[bin_name][0] - bin_depth / 2,
+                default_bin_origins[bin_name][1] - bin_width / 2,
+                bin_height + 0.08]
+            # Allow the origin of the bin to be over-written
+            if 'xyz' in bin_dict:
+                offset_xyz = bin_dict['xyz']
+        else:
+            offset_xyz = get_required_field(bin_name, bin_dict, 'xyz')
+
+        models = get_required_field(bin_name, bin_dict, 'models') or {}
+        for model_type, model_to_spawn_dict in models.items():
+            model_to_spawn_data = {}
+            model_to_spawn_data['type'] = model_type
+            model_to_spawn_data['reference_frame'] = 'world'
+            xyz_start = get_required_field(
+                model_type, model_to_spawn_dict, 'xyz_start')
+            xyz_end = get_required_field(
+                model_type, model_to_spawn_dict, 'xyz_end')
+            rpy = get_required_field(model_type, model_to_spawn_dict, 'rpy')
+            rpy[1] = -bin_angle
+            num_models_x = get_required_field(
+                model_type, model_to_spawn_dict, 'num_models_x')
+            num_models_y = get_required_field(
+                model_type, model_to_spawn_dict, 'num_models_y')
+            step_size = [
+                (xyz_end[0] - xyz_start[0]) / max(1, num_models_x - 1),
+                (xyz_end[1] - xyz_start[1]) / max(1, num_models_y - 1)]
+
+            # Create a grid of models
+            for idx_x in range(num_models_x):
+                for idx_y in range(num_models_y):
+                    model_x_offset = xyz_start[0] + idx_x * step_size[0]
+                    xyz = [
+                        offset_xyz[0] + model_x_offset,
+                        offset_xyz[1] + xyz_start[1] + idx_y * step_size[1],
+                        offset_xyz[2] + xyz_start[2] + model_x_offset * math.tan(bin_angle)]
+                    model_to_spawn_data['pose'] = {'xyz': xyz, 'rpy': rpy}
+                    model_info = create_model_info(model_type, model_to_spawn_data)
+                    # assign each model a unique name because gazebo can't do this
+                    # if the models all spawn at the same time
+                    scoped_model_name = bin_name + '|' + \
+                        model_info.type + '_' + str(get_next_model_id(model_type))
+                    model_info.bin = bin_name
+                    models_to_spawn_infos[scoped_model_name] = model_info
+    return models_to_spawn_infos
+
+
+def create_belt_model_infos(belt_models_dict):
+    belt_model_infos = {}
+    for obj_type, spawn_times in belt_models_dict.items():
+        for spawn_time, belt_model_dict in spawn_times.items():
+            obj_type = replace_type_aliases(obj_type)
+            if obj_type not in belt_model_infos:
+                belt_model_infos[obj_type] = {}
+            belt_model_dict['type'] = obj_type
+            belt_model_infos[obj_type][spawn_time] = create_model_info('belt_model', belt_model_dict)
+    return belt_model_infos
+
+
+def create_drops_info(drops_dict):
+    drops_info = {}
+    drop_region_infos = []
+    drop_regions_dict = get_required_field('drops', drops_dict, 'drop_regions')
+    for drop_name, drop_region_dict in drop_regions_dict.items():
+        frame = get_field_with_default(drop_region_dict, 'frame', 'world')
+        drop_region_min = get_required_field('drop_region', drop_region_dict, 'min')
+        drop_region_min_xyz = get_required_field('min', drop_region_min, 'xyz')
+        drop_region_max = get_required_field('drop_region', drop_region_dict, 'max')
+        drop_region_max_xyz = get_required_field('max', drop_region_max, 'xyz')
+        destination_info = get_required_field('drop_region', drop_region_dict, 'destination')
+        destination = create_pose_info(destination_info)
+        product_type = get_required_field('drop_region', drop_region_dict, 'product_type_to_drop')
+        product_type = replace_type_aliases(product_type)
+        drop_region_infos.append(
+            DropRegionInfo(
+                drop_name, drop_region_min_xyz, drop_region_max_xyz,
+                destination, frame, product_type))
+    drops_info['drop_regions'] = drop_region_infos
+    return drops_info
+
+
+def create_order_info(name, order_dict):
+    shipment_count = get_field_with_default(order_dict, 'shipment_count', 1)
+    destinations = get_field_with_default(order_dict, 'destinations', ["any"] * shipment_count)
+    announcement_condition = get_required_field(name, order_dict, 'announcement_condition')
+    announcement_condition_value = get_required_field(
+        name, order_dict, 'announcement_condition_value')
+    products_dict = get_required_field(name, order_dict, 'products')
+    products = []
+    for product_name, product_dict in products_dict.items():
+        products.append(create_model_info(product_name, product_dict))
+    return {
+        'announcement_condition': announcement_condition,
+        'announcement_condition_value': announcement_condition_value,
+        'products': products,
+        'shipment_count': shipment_count,
+        'destinations': destinations
+    }
+
+
+def create_order_infos(orders_dict):
+    order_infos = {}
+    for order_name, order_dict in orders_dict.items():
+        order_infos[order_name] = create_order_info(order_name, order_dict)
+    return order_infos
+
+
+def create_faulty_products_info(faulty_products_dict):
+    faulty_product_infos = {}
+    for product_name in faulty_products_dict:
+        faulty_product_infos[product_name] = product_name  # no other info for now
+    return faulty_product_infos
+
+
+def create_bin_infos():
+    bin_infos = {}
+    for bin_name, xyz in default_bin_origins.items():
+        bin_infos[bin_name] = PoseInfo(xyz, [0, bin_angle, 3.14159])
+    return bin_infos
+
+
+def create_material_location_info(belt_models, models_over_bins):
+    material_locations = {}
+
+    # Specify that belt products can be found on the conveyor belt
+    for _, spawn_times in belt_models.items():
+        for spawn_time, product in spawn_times.items():
+            if product.type in material_locations:
+                material_locations[product.type].update(['belt'])
+            else:
+                material_locations[product.type] = {'belt'}
+
+    # Specify in which bin the different bin products can be found
+    for product_name, product in models_over_bins.items():
+        if product.type in material_locations:
+            material_locations[product.type].update([product.bin])
+        else:
+            material_locations[product.type] = {product.bin}
+
+    return material_locations
+
+
+def create_options_info(options_dict):
+    options = configurable_options
+    for option, val in options_dict.items():
+        options[option] = val
+    return options
+
+
+def prepare_template_data(config_dict, args):
+    template_data = {
+        'arms': [create_arm_info(name, conf) for name, conf in arm_configs.items()],
+        'sensors': create_sensor_infos(default_sensors, allow_protected_sensors=True),
+        'models_to_insert': {},
+        'models_to_spawn': {},
+        'belt_models': create_belt_model_infos(default_belt_models),
+        'faulty_products': {},
+        'drops': {},
+        'orders': {},
+        # 'options': {'insert_agvs': True},
+        'options': {'insert_agvs': False},
+        'time_limit': default_time_limit,
+        'bin_height': bin_height,
+        'world_dir': world_dir,
+        'joint_limited_ur10': config_dict.pop('joint_limited_ur10', False),
+        'sensor_blackout': {},
+    }
+    # Process the options first as they may affect the processing of the rest
+    options_dict = get_field_with_default(config_dict, 'options', {})
+    template_data['options'].update(create_options_info(options_dict))
+    if args.state_logging is not None:
+        template_data['options']['gazebo_state_logging'] = args.state_logging
+    if args.visualize_sensor_views:
+        template_data['options']['visualize_sensor_views'] = True
+
+    models_over_bins = {}
+    for key, value in config_dict.items():
+        if key == 'sensors':
+            template_data['sensors'].update(
+                create_sensor_infos(value))
+        elif key == 'models_over_bins':
+            models_over_bins = create_models_over_bins_infos(value)
+            template_data['models_to_insert'].update(models_over_bins)
+        elif key == 'belt_models':
+            template_data['belt_models'].update(create_belt_model_infos(value))
+        elif key == 'drops':
+            template_data['drops'].update(create_drops_info(value))
+        elif key == 'faulty_products':
+            template_data['faulty_products'].update(create_faulty_products_info(value))
+        elif key == 'orders':
+            template_data['orders'].update(create_order_infos(value))
+        elif key == 'sensor_blackout':
+            template_data['sensor_blackout'].update(value)
+        elif key == 'options':
+            pass
+        elif key == 'models_to_spawn':
+            template_data['models_to_spawn'].update(
+                create_models_to_spawn_infos(value))
+        elif key == 'time_limit':
+            template_data['time_limit'] = value
+        else:
+            print("Error: unknown top level entry '{0}'".format(key), file=sys.stderr)
+            sys.exit(1)
+    template_data['bins'] = create_bin_infos()
+    template_data['material_locations'] = create_material_location_info(
+        template_data['belt_models'] or {},
+        models_over_bins,
+    )
+    template_data['possible_products'] = possible_products
+    return template_data
+
+
+def generate_files(template_data):
+    files = {}
+    for template_file in template_files:
+        with open(template_file, 'r') as f:
+            data = f.read()
+        files[template_file] = em.expand(data, template_data)
+    # Generate files for each arm
+    for arm_info in template_data['arms']:
+        template_data['arm'] = arm_info
+        with open(arm_template_file, 'r') as f:
+            data = f.read()
+        files[arm_info.name + '.urdf.xacro'] = em.expand(data, template_data)
+    return files
+
+
+def main(sysargv=None):
+    parser = argparse.ArgumentParser(
+        description='Prepares and then executes a gazebo simulation based on configurations.')
+    prepare_arguments(parser)
+    args = parser.parse_args(sysargv)
+    config_data = args.config or ''
+    if args.file is not None:
+        for file in args.file:
+            with open(file, 'r') as f:
+                comp_config_data = f.read()
+                config_data += comp_config_data
+    dict_config = yaml.load(config_data) or {}
+    expanded_dict_config = expand_yaml_substitutions(dict_config)
+    if args.verbose:
+        print(yaml.dump({'Using configuration': expanded_dict_config}))
+
+    random_seed = expanded_dict_config.pop('random_seed', None)
+    initialize_model_id_mappings(random_seed)
+
+    template_data = prepare_template_data(expanded_dict_config, args)
+    files = generate_files(template_data)
+    if not args.dry_run and not os.path.isdir(args.output):
+        if os.path.exists(args.output) and not os.path.isdir(args.output):
+            print('Error, given output directory exists but is not a directory.', file=sys.stderr)
+            sys.exit(1)
+        print('creating directory: ' + args.output)
+        os.makedirs(args.output)
+    for name, content in files.items():
+        if name.endswith('.template'):
+            name = name[:-len('.template')]
+        name = os.path.basename(name)
+        if args.dry_run:
+            print('# file: ' + name)
+            print(content)
+        else:
+            file_path = os.path.join(args.output, name)
+            print('writing file ' + file_path)
+            with open(file_path, 'w+') as f:
+                f.write(content)
+    cmd = [
+        'roslaunch',
+        os.path.join(args.output, 'gear.launch'),
+        'world_path:=' + os.path.join(args.output, 'gear.world'),
+        'gear_urdf_xacro:=' + os.path.join(args.output, 'gear.urdf.xacro'),
+        'arm_urdf_dir:=' + args.output,
+    ]
+    if args.log_to_file:
+        cmd.append('gazebo_ros_output:=log')
+    if args.verbose:
+        cmd += ['verbose:=true']
+    if args.no_gui:
+        cmd += ['gui:=false']
+
+    if not args.development_mode:
+        os.environ['ARIAC_COMPETITION'] = '1'
+
+    print('Running command: ' + ' '.join(cmd))
+    if not args.dry_run:
+        try:
+            p = subprocess.Popen(cmd)
+            p.wait()
+        except KeyboardInterrupt:
+            pass
+        finally:
+            p.wait()
+        return p.returncode
+
+
+if __name__ == '__main__':
+    # Filter out any special ROS remapping arguments.
+    # This is necessary if the script is being run from a ROS launch file.
+    import rospy
+    filtered_argv = rospy.myargv(sys.argv)
+
+    sys.exit(main(filtered_argv[1:]))

src/rrrobot_ws/world/gear.world.template

@@ -0,0 +1,490 @@
+@{
+import em, StringIO, os
+def expand_snippet(filename, data=locals()):
+    output = StringIO.StringIO()
+    interpreter = em.Interpreter(output=output)
+    interpreter.include(os.path.join(world_dir, 'snippet', filename), data)
+    print output.getvalue()
+}@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+  <world name="default">
+    <!-- Set the initial camera pose to be looking at the workspace. -->
+    <gui fullscreen='0'>
+      <camera name='user_camera'>
+        <!-- pretty overview shot -->
+        <pose frame=''>-7.56272 4.06148 6.60308 0 0.636 -0.467992</pose>
+
+        <!-- Top down part placement
+        <pose frame=''>-0.475239 0 4.863623 0 1.570796 0</pose> -->
+
+        <!-- Shipment placement AGV 1
+        <pose frame=''>0.301627 3.242337 2.584521 0 1.570796</pose> -->
+
+        <!-- Shipment placement AGV 2
+        <pose frame=''>0.301627 -3.242337 2.584521 0 1.570796</pose> -->
+
+        <view_controller>orbit</view_controller>
+        <projection_type>perspective</projection_type>
+      </camera>
+    </gui>
+
+@[if options['unthrottled_physics_update']]@
+    <physics type="ode">
+      <real_time_update_rate>0</real_time_update_rate>
+    </physics>
+@[end if]@
+
+@[if not options['disable_shadows']]@
+    <!-- Disable shadows. -->
+    <scene>
+      <shadows>false</shadows>
+    </scene>
+@[end if]@
+
+@[if drops and options['visualize_drop_regions']]@
+@[  for drop_region in drops['drop_regions']]@
+@[    if drop_region.frame == "world"]@
+    <model name="@(drop_region.name)">
+        <pose>@(' '.join([str((float(drop_region.min[i]) + float(drop_region.max[i])) * 0.5) for i in range(3)])) 0 0 0</pose>
+        <static>true</static>
+        <link name="link">
+          <visual name="visual">
+            <geometry>
+            <box>
+              <size>@(' '.join([str(float(drop_region.max[i]) - float(drop_region.min[i])) for i in range(3)]))</size>
+            </box>
+            </geometry>
+          </visual>
+        </link>
+     </model>
+@[    end if]@
+@[  end for]@
+@[end if]@
+
+
+    <!-- A global light source -->
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+
+    <!-- A directed light source -->
+    <light name="camera_spot_light" type='spot'>
+      <pose>14 -3.0 3.0 -1.55 0.0 -1.62</pose>
+      <diffuse>0.5 0.5 0.5 1</diffuse>
+      <specular>0.1 0.1 0.1 1</specular>
+      <direction>0 0 -1</direction>
+      <attenuation>
+        <range>50</range>
+        <constant>0.5</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <cast_shadows>0</cast_shadows>
+      <spot>
+        <inner_angle>0.6</inner_angle>
+        <outer_angle>1</outer_angle>
+        <falloff>1</falloff>
+      </spot>
+    </light>
+
+    <!-- the workcell -->
+    <include>
+      <uri>model://workcell</uri>
+      <pose>0 0 0 0 0 1.57</pose>
+    </include>
+
+@[for bin_name, bin_pose in bins.items()]@
+    <include>
+      <name>@(bin_name)</name>
+      <uri>model://workcell_bin</uri>
+      <pose>@(' '.join(bin_pose.xyz)) @(' '.join(bin_pose.rpy))</pose>
+    </include>
+@[end for]@
+
+@[for name, sensor in sensors.items()]@
+@[if sensor.type == "quality_control"]@
+    <!-- a quality control sensor called @(name) -->
+@{
+expand_snippet('quality_control_sensor.sdf.template')
+}@
+@[end if]@
+
+@[if sensor.type == "break_beam"]@
+    <!-- a break beam sensor called @(name) -->
+@{
+expand_snippet('break_beam.sdf.template')
+}@
+@[end if]@
+
+@[if sensor.type == "proximity_sensor"]@
+    <!-- a proximity sensor called @(name) -->
+@{
+expand_snippet('proximity_sensor.sdf.template')
+}@
+
+@[end if]@
+@[if sensor.type == "logical_camera"]@
+    <!-- a logical camera called @(name) -->
+@{
+expand_snippet('logical_camera.sdf.template')
+}@
+
+@[end if]@
+@[if sensor.type == "depth_camera"]@
+    <!-- a laser profiler called @(name) -->
+@{
+expand_snippet('depth_camera.sdf.template')
+}@
+@[end if]@
+@[if sensor.type == "laser_profiler"]@
+    <!-- a laser profiler called @(name) -->
+@{
+expand_snippet('laser_profiler.sdf.template')
+}@
+@[end if]@
+@[end for]@
+
+    <!-- a wall to delete objects at the end of the belt -->
+    <include>
+      <uri>model://deletion_wall</uri>
+      <pose>1.2 -4.1 1.41 0 0 1.5708</pose>
+    </include>
+
+@{belt_models_loop = True}@
+@{belt_population_cycles = options['belt_population_cycles'] if belt_models_loop else 1}
+
+@{obj_type_index = 0}@
+@[for obj_type, spawn_times in belt_models.items()]@
+    <!-- Pool of @(obj_type) objects -->
+@[  for index in range(belt_population_cycles * len(spawn_times))]@
+    <include>
+      <uri>model://@(obj_type)_ariac</uri>
+      <name>@(obj_type)_@(index)</name>
+      <pose>@(7.5 + 0.25 * obj_type_index) @(-9.8 + 0.25 * index) -5.0 0 0 0</pose>
+    </include>
+@[  end for]@
+@{obj_type_index += 1}@
+@[end for]@
+
+    <!-- an invisible conveyor belt -->
+      <include>
+        <name>conveyor_belt</name>
+        <pose>0.07 0.8 0.464 0 0 0</pose>
+        <uri>model://conveyor_belt_tall_ariac</uri>
+      </include>
+
+@[if options['insert_models_over_bins']]@
+    <!-- Populate the bins -->
+@[for model_name, model in models_to_insert.items()]@
+    <include>
+      <name>@(model_name)</name>
+      <uri>model://@(model.type)_ariac</uri>
+      <pose>@(' '.join(model.pose.xyz)) @(' '.join(model.pose.rpy))</pose>
+    </include>
+@[end for]@
+@[end if]@
+@[if belt_models]@
+
+    <!-- Populate the conveyor belt -->
+    <plugin filename="libROSPopulationPlugin.so" name="populate_conveyor">
+      <activation_topic>/ariac/populate_belt</activation_topic>
+      <rate_modifier_topic>/ariac/population/rate_modifier</rate_modifier_topic>
+      <control_topic>/ariac/population/control</control_topic>
+      <state_topic>/ariac/population/state</state_topic>
+      <start_index>0</start_index>
+      <prefix_object_names>false</prefix_object_names>
+      <loop_forever>@("true" if belt_models_loop else "false")</loop_forever>
+      <frame>conveyor_belt::conveyor_belt_fixed</frame>
+      <object_sequence>
+@[for product_name, spawn_times in belt_models.items()]@
+  @[for spawn_time, product in spawn_times.items()]@
+        <object>
+          <time>@(spawn_time)</time>
+          <type>@(product.type)</type>
+          <pose>@(' '.join(product.pose.xyz)) @(' '.join(product.pose.rpy))</pose>
+        </object>
+  @[end for]@
+@[end for]@
+      </object_sequence>
+      <update_rate>10</update_rate>
+    </plugin>
+@[end if]@
+
+@[if options['insert_agvs']]@
+@[for agv_id in [1,2]]@
+    <!-- AGV@(str(agv_id)) -->
+    <model name="agv@(str(agv_id))">
+      <pose>0.3 @(3.3 if agv_id == 1 else -3.3) 0 0 0 @(3.14159 if agv_id == 1 else 0)</pose>
+      <include>
+        <name>agv@(str(agv_id))</name>
+        <uri>model://warehouse_robot_ariac</uri>
+      </include>
+      <plugin name="agv_plugin" filename="libROSAGVPlugin.so">
+        <agv_control_service_name>/ariac/agv@(str(agv_id))/animate</agv_control_service_name>
+        <clear_tray_service_name>/ariac/kit_tray_@(str(agv_id))/clear_tray</clear_tray_service_name>
+        <lock_tray_service_name>/ariac/kit_tray_@(str(agv_id))/lock_models</lock_tray_service_name>
+        <index>@(str(agv_id))</index>
+      </plugin>
+
+      <!-- a tray for building kits -->
+      <!-- (this has to be in a model tag so logical cameras can detect it as a nested model) -->
+      <model name="kit_tray_@(str(agv_id))">
+        <pose>0.0 0.15 0.75 0 0 0</pose>
+        <include>
+          <name>kit_tray_@(str(agv_id))</name>
+          <uri>model://kit_tray_ariac</uri>
+        </include>
+        <plugin name="kit_tray_plugin" filename="libROSAriacKitTrayPlugin.so">
+          <contact_sensor_name>kit_tray_contact</contact_sensor_name>
+          <tf_frame_name>kit_tray_@(str(agv_id))</tf_frame_name>
+          <clear_tray_service_name>/ariac/kit_tray_@(str(agv_id))/clear_tray</clear_tray_service_name>
+          <lock_models_service_name>/ariac/kit_tray_@(str(agv_id))/lock_models</lock_models_service_name>
+          <get_content_service_name>/ariac/kit_tray_@(str(agv_id))/get_content</get_content_service_name>
+          <update_rate>20</update_rate>
+          <faulty_parts>
+@[for part_name, part_info in faulty_products.items()]@
+            <name>@(part_name)</name>
+@[end for]@
+          </faulty_parts>
+        </plugin>
+      </model>
+
+      <!-- join the tray and agv -->
+      <joint name="agv_tray" type="fixed">
+          <parent>agv@(str(agv_id))::link</parent>
+          <child>kit_tray_@(str(agv_id))::kit_tray_@(str(agv_id))::tray</child>
+      </joint>
+    </model>
+
+    <model name="quality_control_sensor_@(str(agv_id))">
+      <pose>0.3 @(3.5 if agv_id == 1 else -3.5) 1.5 0 1.574 @(-1.574 if agv_id == 1 else 1.574)</pose>
+      <plugin name="ros_logical_camera" filename="libROSLogicalCameraPlugin.so">
+        <robotNamespace>ariac</robotNamespace>
+        <position_noise>
+          <noise>
+            <type>gaussian</type>
+            <mean>0.0</mean>
+            <stddev>0.001</stddev>
+          </noise>
+        </position_noise>
+        <orientation_noise>
+          <noise>
+            <type>gaussian</type>
+            <mean>0.0</mean>
+            <stddev>0.01</stddev>
+          </noise>
+        </orientation_noise>
+        <known_model_names>
+@[for part_name, part_info in faulty_products.items()]@
+          <name>@(part_name)</name>
+@[end for]@
+        </known_model_names>
+      </plugin>
+      <link name="link">
+        <gravity>false</gravity>
+        <inertial>
+          <mass>0.1</mass>
+          <inertia>
+            <ixx>0.000166667</ixx>
+            <iyy>0.000166667</iyy>
+            <izz>0.000166667</izz>
+          </inertia>
+        </inertial>
+
+        <collision name="collision">
+          <pose>0 0.02 0 0 0 -1.5708</pose>
+          <geometry>
+            <mesh>
+              <uri>model://logical_camera/meshes/camera.dae</uri>
+            </mesh>
+          </geometry>
+        </collision>
+
+        <visual name="visual">
+          <pose>0 0.02 0 0 0 -1.5708</pose>
+          <geometry>
+            <mesh>
+              <uri>model://logical_camera/meshes/camera.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+
+        <sensor name="logical_camera" type="logical_camera">
+          <logical_camera>
+            <near>0.73</near>
+            <far>0.78</far>
+            <horizontal_fov>0.7</horizontal_fov>
+            <aspect_ratio>0.35</aspect_ratio>
+          </logical_camera>
+
+          <visualize>false</visualize>
+          <always_on>true</always_on>
+          <update_rate>10</update_rate>
+        </sensor>
+      </link>
+    </model>
+
+@[end for]@
+@[end if]@
+
+
+    <!-- The NIST-ARIAC task manager -->
+    <plugin filename="libROSAriacTaskManagerPlugin.so" name="task_manager">
+      <robot_namespace>ariac</robot_namespace>
+      <competition_time_limit>@(time_limit)</competition_time_limit>
+      <start_competition_service_name>/ariac/start_competition</start_competition_service_name>
+      <end_competition_service_name>/ariac/end_competition</end_competition_service_name>
+      <population_activate_topic>/ariac/populate_belt</population_activate_topic>
+      <conveyor_control_service>/ariac/conveyor/control</conveyor_control_service>
+      <submit_tray_service_name>/ariac/submit_shipment</submit_tray_service_name>
+      <material_locations_service_name>/ariac/material_locations</material_locations_service_name>
+      <shipment_content_topic_name>/ariac/trays</shipment_content_topic_name>
+      <orders_topic>/ariac/orders</orders_topic>
+@[for agv_id in [1,2]]@
+      <agv index="@(agv_id)">
+        <agv_control_service_name>/ariac/agv@(agv_id)</agv_control_service_name>
+        <agv_animate_service_name>/ariac/agv@(agv_id)/animate</agv_animate_service_name>
+        <get_content_service_name>/ariac/kit_tray_@(agv_id)/get_content</get_content_service_name>
+      </agv>
+@[end for]@
+@[for order_name, order in orders.items()]@
+      <order>
+        <name>@(order_name)</name>
+@[if order['announcement_condition'] == 'time']@
+        <start_time>@(order['announcement_condition_value'])</start_time>
+@[end if]@
+@[if order['announcement_condition'] == 'wanted_products']@
+        <interrupt_on_wanted_products>@(order['announcement_condition_value'])</interrupt_on_wanted_products>
+@[end if]@
+@[if order['announcement_condition'] == 'unwanted_products']@
+        <interrupt_on_unwanted_products>@(order['announcement_condition_value'])</interrupt_on_unwanted_products>
+@[end if]@
+@[for shipment_index in range(order['shipment_count'])]
+@{
+if '_update' in order_name:
+    order_name = order_name.split('_update')[0]
+destination = order['destinations'][shipment_index]
+}@
+        <shipment>
+          <shipment_type>@(order_name)_shipment_@(shipment_index)</shipment_type>
+          <destination>@(destination)</destination>
+@[for product in order['products']]@
+          <product>
+            <type>@(product.type)</type>
+            <pose>@(' '.join(product.pose.xyz)) @(' '.join(product.pose.rpy))</pose>
+          </product>
+@[end for]@
+        </shipment>
+@[end for]@
+      </order>
+@[end for]@
+      <material_locations>
+@[for material_name, locations in material_locations.items()]@
+        <material>
+          <type>@(material_name)</type>
+@[for location in locations]@
+          <location>
+            <storage_unit>@(location)</storage_unit>
+          </location>
+@[end for]@
+        </material>
+@[end for]@
+      </material_locations>
+@[if sensor_blackout]@
+      <sensor_blackout>
+        <duration>@(sensor_blackout['duration'])</duration>
+        <product_count>@(sensor_blackout['product_count'])</product_count>
+        <topic>/ariac/sensor_enable</topic>
+      </sensor_blackout>
+@[end if]@
+      <update_rate>10</update_rate>
+
+    </plugin>
+
+    <!-- Plane under the workcell on which 'removed' models can be placed -->
+    <model name="under_workcell_plane">
+      <pose>0 0 -5.1 0 0 0</pose>
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>25 25</size>
+            </plane>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>100</mu>
+                <mu2>50</mu2>
+              </ode>
+            </friction>
+          </surface>
+        </collision>
+      </link>
+    </model>
+
+    <!-- A ground plane to hide the under_workcell_plane -->
+    <model name="ground_plane">
+      <static>true</static>
+      <pose>-50 50 -0.01 0 0 0</pose>
+      <link name="link">
+        <visual name="visual">
+          <cast_shadows>false</cast_shadows>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>150 150</size>
+            </plane>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <!-- A visual and collision of the linear rail the robots ride on -->
+    <model name="linear_rail">
+      <pose>0.3 0 0.9 0 0 0</pose>
+      <static>true</static>
+      <link name="rail_link">
+        <visual name="rail_link_visual">
+          <geometry>
+            <box>
+              <size>0.2 4.6 0.1</size>
+            </box>
+          </geometry>
+          <material>
+            <script>Gazebo/Grey</script>
+          </material>
+        </visual>
+
+        <collision name="rail_link_collision">
+          <geometry>
+            <box>
+              <size>0.2 4.6 0.1</size>
+            </box>
+          </geometry>
+        </collision>
+      </link>
+    </model>
+
+    <plugin name='insert_models' filename='libinsert_model.so'/>
+  </world>
+</sdf>

src/rrrobot_ws/world/rrrobot_setpoint.world

@@ -0,0 +1,68 @@
+<sdf version='1.6'>
+  <world name='default'>
+    <model name='ground_plane'>
+      <static>1</static>
+      <link name='link'>
+        <collision name='collision'>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <surface>
+            <contact>
+              <collide_bitmask>65535</collide_bitmask>
+              <ode/>
+            </contact>
+            <friction>
+              <ode>
+                <mu>100</mu>
+                <mu2>50</mu2>
+              </ode>
+              <torsional>
+                <ode/>
+              </torsional>
+            </friction>
+            <bounce/>
+          </surface>
+          <max_contacts>10</max_contacts>
+        </collision>
+        <visual name='visual'>
+          <cast_shadows>0</cast_shadows>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+    </model>
+    <light name='sun' type='directional'>
+      <cast_shadows>1</cast_shadows>
+      <pose frame=''>0 0 10 0 -0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+    <include>
+      <uri>model://fanuc_robotic_arm_with_gripper</uri>
+    </include>
+  </world>
+</sdf>