*

.env

+UID=1000
+GID=1000

.gitignore

+cmake-build-debug
+.catkin_tools/
+.catkin_workspace
+build/
+devel/
+logs/
+*.swp
+.idea
+src/CMakeLists.txt

.gitlab-ci.yml

+variables:
+  GIT_SUBMODULE_STRATEGY: recursive
+
+stages:
+  - build
+
+build-ws:
+  image: ros:melodic-ros-base-bionic
+  stage: build
+  script:
+    - apt-get update
+    - apt-get install -y ros-melodic-libfranka python-catkin-tools libeigen3-dev 
+    - echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc
+    - source ~/.bashrc
+    - rosdep install -y --ignore-src --from-paths .
+    - catkin build
+  tags:
+  - highmem
+

.gitmodules

+[submodule "src/franka_description"]
+	path = src/franka_description
+	url = ../franka_description.git
+[submodule "src/panda_moveit_config"]
+	path = src/panda_moveit_config
+	url = ../panda_moveit_config.git
+[submodule "src/panda_simulation"]
+	path = src/panda_simulation
+	url = ../panda_simulation.git
+[submodule "src/panda_mqtt_connector"]
+	path = src/panda_mqtt_connector
+	url = ../panda_mqtt_connector.git
+[submodule "ros2rag"]
+	path = ros2rag
+	url = ../../../jastadd/ros2rag.git
+[submodule "src/simulation_util"]
+	path = src/simulation_util
+	url = ../simulation_util.git
+	branch = master

Dockerfile

+FROM osrf/ros:melodic-desktop-full-bionic
+
+ARG uid=1000
+ARG gid=1000
+
+COPY entrypoint.sh /entrypoint.sh
+RUN chmod +x /entrypoint.sh
+
+ENV DEBIAN_FRONTEND noninteractive
+ENV USERNAME ros
+RUN useradd -m $USERNAME && \
+        echo "$USERNAME:$USERNAME" | chpasswd && \
+        usermod --shell /bin/bash $USERNAME && \
+        usermod -aG sudo $USERNAME && \
+        echo "$USERNAME ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers.d/$USERNAME && \
+        chmod 0440 /etc/sudoers.d/$USERNAME && \
+        usermod  --uid $uid $USERNAME && \
+        groupmod --gid $gid $USERNAME && \
+        usermod -aG video $USERNAME
+
+RUN apt-get update && apt-get upgrade -y && \
+        apt-get install -y \
+          python-catkin-tools \
+          bash \
+          mesa-utils \
+          libgl1-mesa-glx \
+          apt-utils
+WORKDIR /home/$USERNAME
+COPY src /home/$USERNAME/src
+RUN rosdep install --from-paths . --ignore-src -r -y
+RUN catkin config \
+      --extend /opt/ros/melodic && \
+    catkin build
+
+ENTRYPOINT ["/entrypoint.sh"]

README.md

+# A Complete Panda Gazebo Workspace
+
+## Important Remarks:
+
+- Much of the code in the contained submodules is based on the information [Erdal Pekel's Blog](https://erdalpekel.de/). **Thanks, Erdal!**
+- The state outside of the master branch must be considered as "work-in-progress". Only the master branch is stable here.
+- Currently, only very simple use cases are contained here. We will gradually update the code. Watch this space!
+- The examples only work when *all* projects in this workspace are checked out.
+
+## Docker
+
+To quickly run the graphical example applications, you can use Docker. For development purposes, please use the regular installation
+instructions further below in this document. Please note that the setup for Windows and Mac OS systems uses X11 forwarding and thus
+offers limited graphics performance.
+
+- Clone this repo and its submodules:
+    - as a guest: `git clone --recurse-submodules https://git-st.inf.tu-dresden.de/ceti/ros-interal/panda_gazebo_workspace.git panda_gazebo_workspace`
+    - as a project member with a registered ssh key: `git clone --recurse-submodules git@git-st.inf.tu-dresden.de:ceti/ros-interal/panda_gazebo_workspace.git panda_gazebo_workspace`
+- Follow the steps for your operating system.
+
+### Linux
+
+- Install Docker for your linux distribution
+- Run `./docker-run.sh roslaunch sample_applications sample_simple_simulation.launch` to build and run a docker image tagged with `ceti-panda-gazebo-workspace`
+
+### Windows 10
+
+- Install [Docker Desktop for Windows](https://hub.docker.com/editions/community/docker-ce-desktop-windows)
+- Install [Xming](https://sourceforge.net/projects/xming/)
+- Run `docker-run.bat roslaunch sample_applications sample_simple_simulation.launch`
+
+### Mac OS
+
+- Install [Docker Destop for Mac OS](https://hub.docker.com/editions/community/docker-ce-desktop-mac)
+- Install [XQuartz](https://www.xquartz.org/). Enable the `Allow connections from network clients` setting in the preferences (`Security` Tab).
+- Run `docker-run-mac.sh roslaunch sample_applications sample_simple_simulation.launch`
+
+## Requirements for development
+
+- Ubuntu 18.4
+- ROS Melodic
+
+## Installation
+
+- There are two ways to install, using git submodules or by cloning the required ros packages individually.
+    - **Either** clone this repo and its submodules (recommended for fresh installations):
+        - as a guest: `git clone --recurse-submodules https://git-st.inf.tu-dresden.de/ceti/ros/panda_gazebo_workspace.git panda_gazebo_workspace`
+        - as a project member with a registered ssh key: `git clone --recurse-submodules git@git-st.inf.tu-dresden.de:ceti/ros/panda_gazebo_workspace.git panda_gazebo_workspace`
+- change into the workspace `cd panda_gazebo_workspace`
+    - **Or** clone the provided packages individually (recommended for existing ROS workspaces or if you do not understand or like submodules)
+        - if you do not have a catkin workspace yet, create one `mkdir -p panda_gazebo_workspace/src`
+        - change into a `src` directory of a catkin workspace, e.g. with `cd panda_gazebo_workspace/src`
+        - clone the four provided packages into the `src` directory (use https if you are not registered or do not have configured an ssh key yet)
+        - `git clone https://git-st.inf.tu-dresden.de/ceti/ros/franka_description.git` **or** `git clone git@git-st.inf.tu-dresden.de:ceti/ros/franka_description.git`
+        - `git clone https://git-st.inf.tu-dresden.de/ceti/ros/panda_moveit_config.git` **or** `git clone git@git-st.inf.tu-dresden.de:ceti/ros/panda_moveit_config.git`
+        - `git clone https://git-st.inf.tu-dresden.de/ceti/ros/panda_simulation.git` **or** `git clone git@git-st.inf.tu-dresden.de:ceti/ros/panda_simulation.git`
+        - `git clone https://git-st.inf.tu-dresden.de/ceti/ros/sample_applications.git` **or** `git clone git@git-st.inf.tu-dresden.de:ceti/ros/sample_applications.git`
+        - change into the workspace main directory `cd ..`
+- install ROS package dependencies `rosdep install --from-paths .`
+- build the workspace `catkin build`
+- source the config: depending on your shell
+    - `source devel/setup.bash` (default)
+    - `source devel/setup.sh`
+    - `source devel/setup.zsh`
+
+## Simulations
+
+See the [README.md of the submodule](../../../../sample_applications/-/blob/master/README.md) for more information.
+
+- For planning and simulation based on rviz: `roslaunch panda_simulation simulation.launch`
+- Execution of a simple motion: `roslaunch sample_applications sample_simple_simulation.launch`
+- Execution of a simple motion costraint by a blocking object: `roslaunch sample_applications sample_constraint_simulation.launch`
+- Execution of a velocity constraint cartesian trajectory: `roslaunch sample_applications simulation.launch`

config.xlaunch

+<?xml version="1.0"?>
+<XLaunch xmlns="http://www.straightrunning.com/XmingNotes" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:schemaLocation="http://www.straightrunning.com/XmingNotes XLaunch.xsd" WindowMode="MultiWindow" ClientMode="NoClient" Display="0" Clipboard="true" NoAccessControl="true"/>

config/config.yaml

+panda_mqtt_connector:
+  server: "tcp://localhost:1883"
+  robot_speed_factor: .7
+  robot_planning_mode: "fluid_path" # "fluid_path" or "cartesian_path"
+  default_trajectory: "<none>" # "square" or "circle", everything else is ignored
+  data_publish_rate: 100.0          # publication frequency of the robot state message
+  topics:
+    robotConfig: "robotconfig"
+    trajectory: "trajectory"
+    nextStep: "ros2rag/nextStep"
+  zone_size: 0.5
+  zones:
+    - "1 1"
+    - "0 1"
+    - "-1 1"
+  parts:
+    panda:
+      Link0: "panda_link0"
+      Link1: "panda_link1"
+      Link2: "panda_link2"
+      Link3: "panda_link3"
+      Link4: "panda_link4"
+      Link5: "panda_link5"
+      Link6: "panda_link6"
+      RightFinger: "panda_rightfinger"
+      LeftFinger: "panda_leftfinger"
+  end_effectors:
+    panda:
+      EndEffector: "panda_hand"
+  goal_poses:
+    - position: "0.4 0.4 0.3"
+      orientation: "1 1 0 0"
+      work: "20000"
+    - position: "-0.4 0.4 0.3"
+      orientation: "1 1 0 0"
+      work: "20000"
+    - position: "-0.4 -0.4 0.3"
+      orientation: "1 1 0 0"
+      work: "20000"
+    - position: "0.4 -0.4 0.3"
+      orientation: "1 1 0 0"
+      work: "20000"
+    - position: "0.4 -0.4 0.3" # stay at the position for a while longer
+      orientation: "1 1 0 0"
+      work: "20000"
+    - position: "0.4 0.4 0.3"  # move into the safety zone again to print the line in the plot
+      orientation: "1 1 0 0"
+      work: "20000"

docker-build.sh

+#!/bin/bash
+
+# http://wiki.ros.org/docker/Tutorials/GUI
+
+docker build . -t ceti-panda-gazebo-workspace \
+    --build-arg UID=$UID \
+    --build-arg GID=$GID
+

docker-compose.yml

+version: '3'
+services:
+  ros:
+    build:
+      context: .
+      args:
+        - "uid=${UID}"
+        - "gid=${GID}"
+    network_mode: host
+    # depends_on:
+    #   - mosquitto
+    volumes:
+      - "/tmp/.X11-unix:/tmp/.X11-unix:rw"
+      - "/tmp/.docker.xauth:/tmp/.docker.xauth:rw"
+      # - "config/config.yaml:/home/ros/src/panda_mqtt_connector/config/config.yaml"
+    environment:
+      - "XAUTHORITY=/tmp/.docker.xauth"
+      - "DISPLAY=$DISPLAY"
+    devices:
+      - "/dev/dri"
+    user: "ros"
+    entrypoint: ['/entrypoint.sh', 'roslaunch', 'panda_mqtt_connector', 'simulation_rosrag_test.launch']
+
+  rag_app:
+    build:
+      context: ros2rag
+    network_mode: host
+    environment:
+      - "TARGET=starter"
+      - "CONFIG_FILE=/config/config.yaml"
+    volumes:
+      - "./config/config.yaml:/config/config.yaml"
+
+  rag_goal:
+    build:
+      context: ros2rag
+    network_mode: host
+    environment:
+      - "TARGET=goal"
+      - "CONFIG_FILE=/config/config.yaml"
+    volumes:
+      - "./config/config.yaml:/config/config.yaml"
+
+  mosquitto:
+    image: eclipse-mosquitto
+    container_name: mqtt
+    network_mode: host
+    restart: always
+    volumes:
+      - ./docker/mosquitto.conf:/mosquitto/config/mosquitto.conf
+    ports:
+      - "1883:1883"
+      - "9001:9001"

docker-run-mac.sh

+#!/bin/sh
+
+docker build . -t ceti-panda-gazebo-workspace
+
+XSOCK=/tmp/.X11-unix
+
+/usr/X11/bin/xhost + 127.0.0.1
+defaults write org.macosforge.xquartz.X11 enable_iglx -bool true
+
+docker run -it \
+    --volume=$XSOCK:$XSOCK:rw \
+    --env="LIBGL_ALWAYS_INDIRECT=1" \
+    --env="DISPLAY=host.docker.internal:0" \
+    --user="ros" \
+    ceti-panda-gazebo-workspace \
+    "$@"
\ No newline at end of file

docker-run-ros2rag.sh

+#!/bin/bash
+
+# http://wiki.ros.org/docker/Tutorials/GUI
+
+XSOCK=/tmp/.X11-unix
+XAUTH=/tmp/.docker.xauth
+touch $XAUTH
+xauth nlist $DISPLAY | sed -e 's/^..../ffff/' | xauth -f $XAUTH nmerge -
+
+docker run -it \
+    --volume=$XSOCK:$XSOCK:rw \
+    --volume=$XAUTH:$XAUTH:rw \
+    --env="XAUTHORITY=${XAUTH}" \
+    --env="DISPLAY" \
+    --device=/dev/dri \
+    --group-add video \
+    --user="ros" \
+    -p 1883 \
+    ceti-panda-gazebo-workspace \
+    roslaunch panda_mqtt_connector simulation_rosrag.launch

docker-run.bat

+start config.xlaunch
+
+docker build . -t ceti-panda-gazebo-workspace
+
+docker run -it^
+ --env="DISPLAY=host.docker.internal:0"^
+ --user="ros"^
+ ceti-panda-gazebo-workspace^
+ %*
+
+pause
\ No newline at end of file

docker-run.sh

+#!/bin/bash
+
+# http://wiki.ros.org/docker/Tutorials/GUI
+
+./docker-build.sh
+
+XSOCK=/tmp/.X11-unix
+XAUTH=/tmp/.docker.xauth
+touch $XAUTH
+xauth nlist $DISPLAY | sed -e 's/^..../ffff/' | xauth -f $XAUTH nmerge -
+
+docker run -it \
+    --volume=$XSOCK:$XSOCK:rw \
+    --volume=$XAUTH:$XAUTH:rw \
+    --env="XAUTHORITY=${XAUTH}" \
+    --env="DISPLAY" \
+    --device=/dev/dri \
+    --group-add video \
+    --user="ros" \
+    -p 1883 \
+    ceti-panda-gazebo-workspace \
+    "$@"

entrypoint.sh

+#!/bin/bash
+source /opt/ros/melodic/setup.bash
+source $HOME/devel/setup.bash
+exec "$@"
\ No newline at end of file

measurements/filter.sh

+rosbag filter rqt_multiplot-paper-run-6.bag rqt_multiplot-paper-run-6-filtered.bag "((t.secs == 11 and t.nsecs <= 60000000000) or t.secs > 11) and ((t.secs == 51 and t.nsecs <= 145000000) or t.secs < 51)"

measurements/filter.txt

+ rosbag info rqt_multiplot-paper-run-6-filtered.bag 
+path:        rqt_multiplot-paper-run-6-filtered.bag
+version:     2.0
+duration:    40.1s
+start:       Jan 01 1970 01:00:11.01 (11.01)
+end:         Jan 01 1970 01:00:51.14 (51.14)
+size:        5.5 MB
+messages:    40906
+compression: none [7/7 chunks]
+types:       geometry_msgs/PoseStamped            [d3812c3cbc69362b77dc0b19b345f8f5]
+             panda_mqtt_connector/MqttMessage     [34d4a0fecc4246bd71e701fec8c394e4]
+             panda_mqtt_connector/StampedInZone   [86a2ed390383ecbe46c2e0ff42717057]
+             panda_mqtt_connector/StampedVelocity [efabe4b9cc457f55707b4740c9f2a756]
+             panda_mqtt_connector/Trajectory      [431dc3f61a2135dc3f1b031b78206179]
+             std_msgs/Float64                     [fdb28210bfa9d7c91146260178d9a584]
+             std_msgs/Int16                       [8524586e34fbd7cb1c08c5f5f1ca0e57]
+topics:      /panda_mqtt_connector/max_velocity           6 msgs    : std_msgs/Float64                    
+             /panda_mqtt_connector/mqtt_message           9 msgs    : panda_mqtt_connector/MqttMessage    
+             /panda_mqtt_connector/next_step              9 msgs    : std_msgs/Int16                      
+             /panda_mqtt_connector/panda/EndEffector   3881 msgs    : geometry_msgs/PoseStamped           
+             /panda_mqtt_connector/panda/LeftFinger    3881 msgs    : geometry_msgs/PoseStamped           
+             /panda_mqtt_connector/panda/Link0         3881 msgs    : geometry_msgs/PoseStamped           
+             /panda_mqtt_connector/panda/Link1         3881 msgs    : geometry_msgs/PoseStamped           
+             /panda_mqtt_connector/panda/Link2         3881 msgs    : geometry_msgs/PoseStamped           
+             /panda_mqtt_connector/panda/Link3         3881 msgs    : geometry_msgs/PoseStamped           
+             /panda_mqtt_connector/panda/Link4         3881 msgs    : geometry_msgs/PoseStamped           
+             /panda_mqtt_connector/panda/Link5         3881 msgs    : geometry_msgs/PoseStamped           
+             /panda_mqtt_connector/panda/Link6         3881 msgs    : geometry_msgs/PoseStamped           
+             /panda_mqtt_connector/panda/RightFinger   3881 msgs    : geometry_msgs/PoseStamped           
+             /panda_mqtt_connector/robot_in_zone         44 msgs    : panda_mqtt_connector/StampedInZone  
+             /panda_mqtt_connector/trajectory_update      4 msgs    : panda_mqtt_connector/Trajectory     
+             /panda_mqtt_connector/velocity            1012 msgs    : panda_mqtt_connector/StampedVelocity
+             /panda_mqtt_connector/velocity_smoothed   1012 msgs    : panda_mqtt_connector/StampedVelocity
+