diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json
index d5d2a41..0871c97 100644
--- a/.devcontainer/devcontainer.json
+++ b/.devcontainer/devcontainer.json
@@ -6,7 +6,7 @@
     //         "WORKSPACE": "${containerWorkspaceFolder}"
     //     }
     // },
-    "image": "irth7/ros2aria-dev",
+    "image": "delicjusz/ros2aria",
     "remoteUser": "ros",
     "runArgs": [
         "--network=host",
diff --git a/Dockerfile b/Dockerfile
index 36985d5..ce82db1 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -1,24 +1,33 @@
 ARG ROS_DISTRO=humble
-FROM ros:$ROS_DISTRO
-
+FROM delicjusz/ros2aria 
 SHELL ["/bin/bash", "-c"]
-RUN apt-get update && \
-    # apt-get install -y  \
-    # && \
-    apt-get autoremove -y && \
-    apt-get clean && \
-    rm -rf /var/lib/apt/lists/*
+RUN apt-get update && apt-get install -y \
+		git \
+        python3-colcon-common-extensions \
+        python3-rosdep \
+		build-essential
+
+WORKDIR /ros2_ws
+
+COPY src/ /ros2_ws/src/
+RUN source /opt/ros/$ROS_DISTRO/setup.bash && \
+    rm -rf /etc/ros/rosdep/sources.list.d/20-default.list && \
+    rosdep init && \
+    rosdep update --rosdistro $ROS_DISTRO && \
+    rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y && \
+    colcon build
 
-COPY ./src/AriaCoda /usr/local/Aria
-RUN cd /usr/local/Aria && make -j$(nproc)
-ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/Aria/lib
 
 RUN echo "source /opt/ros/$ROS_DISTRO/setup.bash" >> ~/.bashrc
-COPY ros_entrypoint.sh /
-ENTRYPOINT ["../ros_entrypoint.sh"]
-RUN chmod +x /ros_entrypoint.sh
+# COPY ros_entrypoint.sh /
+# ENTRYPOINT ["../ros_entrypoint.sh"]
+# RUN chmod +x /ros_entrypoint.sh
 
-WORKDIR /app
-COPY src/ros2aria /app/src/ros2aria
-COPY src/ros2aria_msgs /app/src/ros2aria_msgs
-RUN cd /app && source /opt/ros/$ROS_DISTRO/setup.bash && colcon build
\ No newline at end of file
+# clear ubuntu packages
+# RUN	export SUDO_FORCE_REMOVE=yes && \
+#     apt-get clean && \
+# 	apt-get remove -y \
+# 		python3-colcon-common-extensions \
+# 		python3-rosdep \
+# 		build-essential && \
+# 	rm -rf /var/lib/apt/lists/*
\ No newline at end of file
diff --git a/Dockerfile.aria b/Dockerfile.aria
new file mode 100644
index 0000000..0f01ab1
--- /dev/null
+++ b/Dockerfile.aria
@@ -0,0 +1,19 @@
+ARG ROS_DISTRO=humble
+FROM husarnet/ros:humble-ros-core
+ARG ROS_DISTRO
+
+SHELL ["/bin/bash", "-c"]
+
+RUN mkdir -p /ros2_ws/src && cd /ros2_ws/src
+WORKDIR /ros2_ws
+
+RUN apt-get update && apt-get install -y \
+		build-essential
+
+COPY ./src/AriaCoda /usr/local/Aria
+RUN cd /usr/local/Aria && make -j$(nproc) && export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/Aria
+	# clear ubuntu packages
+RUN	apt-get clean && \
+	apt-get remove -y \
+		build-essential && \
+	rm -rf /var/lib/apt/lists/*
\ No newline at end of file
diff --git a/Makefile b/Makefile
index 9da983f..3b84ac9 100644
--- a/Makefile
+++ b/Makefile
@@ -10,13 +10,24 @@ build: build/ros2aria/ros2aria
 
 # .uploaded: build/ros2aria/ros2aria
 upload:
-	rsync -r  . lab1_5@pionier5:~/ros2aria
+	rsync -r  . lab1_5@pionier2:~/ros2aria
 	touch .uploaded
 
 # upload: .uploaded
 
 run: upload
-	ssh lab1_5@pionier6 -t -- docker run --rm --network=host -it --device /dev/ttyS0 -v /home/lab1_5:/ws irth7/ros2aria-dev /bin/bash /ws/ros2aria/run.sh
+	ssh lab1_5@pionier2 -t -- docker run --rm --network=host -it --device /dev/ttyS0  delicjusz/ros2aria /bin/bash /ros2_ws/ros2aria/run.sh
+# export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/Aria/lib ros2 run ros2aria ros2aria --ros-args -p pioneer_id:=2
 
 legacy:
-	ssh lab1_5@pionier6 -t -- ./run.sh
+	ssh lab1_5@pionier2 -t -- ./run.sh
+
+push_ros2aria:
+	./build_devcontainer.sh
+	docker push delicjusz/ros2aria
+
+push:
+	docker build -f Dockerfile -t delicjusz/pioneer .
+	docker push delicjusz/pioneer
+pull:
+	docker pull delicjusz/ros2aria
\ No newline at end of file
diff --git a/README.md b/README.md
new file mode 100644
index 0000000..4333b71
--- /dev/null
+++ b/README.md
@@ -0,0 +1,21 @@
+# Instruction on station
+Export discovery server configuration file:
+```bash
+export FASTRTPS_DEFAULT_PROFILES_FILE=$(pwd)/fastdds.xml
+```
+
+Export discovery server ip:
+```
+export ROS_DISCOVERY_SERVER=10.104.16.240:11811
+```
+
+Export fastdds ros middleware implementation:
+```
+export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
+```
+
+Restart ROS 2 daemon:
+```
+ros2 daemon stop
+ros2 daemon start
+```
\ No newline at end of file
diff --git a/build_devcontainer.sh b/build_devcontainer.sh
index 4a269e0..b3a7ccf 100755
--- a/build_devcontainer.sh
+++ b/build_devcontainer.sh
@@ -1,3 +1,3 @@
 #!/bin/sh
-docker build --build-arg WORKSPACE=/ws -f Dockerfile.devcontainer -t irth7/ros2aria-dev .
+docker build -f Dockerfile.aria -t delicjusz/ros2aria .
 
diff --git a/code.py b/code.py
new file mode 100644
index 0000000..207f502
--- /dev/null
+++ b/code.py
@@ -0,0 +1,33 @@
+    try:
+        cl, addr = s.accept()
+        print('client connected from', addr)
+        request = cl.recv(1024)
+        request = str(request)
+        print("request: {}".format(request))
+        led_on = request.find('/dioda/on')
+        led_off = request.find('/dioda/off')
+        text_index = request.find('/text')
+
+        print( 'text = ' + str(text_index))
+        print( 'led on = ' + str(led_on))
+        print( 'led off = ' + str(led_off))
+        if text_index == 6:
+            entrypoint_text = request[text_index+len('/text/'):]
+            text = entrypoint_text[:entrypoint_text.find(' ')]
+            print('Hello world {}'.format(text))
+        if led_on == 6:
+            led.on()
+        else:
+            led.off()
+            
+ 
+        
+        response = html
+        response = html % stateis
+        cl.send('HTTP/1.0 200 OK\r\nContent-type: text/html\r\n\r\n')
+        cl.send(response)
+        cl.close()
+
+    except OSError as e:
+        cl.close()
+        print('connection closed')
diff --git a/compose.foxglove.yaml b/compose.foxglove.yaml
new file mode 100644
index 0000000..3ad6ada
--- /dev/null
+++ b/compose.foxglove.yaml
@@ -0,0 +1,27 @@
+x-net-config:
+  &net-config
+  network_mode: host
+  ipc: host
+
+services:
+  foxglove:
+    image: husarion/foxglove:humble-1.39.1-20230220
+    ports:
+      - 8080:8080
+    volumes:
+      - ./config/rosbot_sensors_foxglove.json:/src/FoxgloveDefaultLayout.json
+    environment:
+      - FOXGLOVE_PORT=8080
+      - ROSBRIDGE_PORT=9090
+
+  rosbridge:
+    image: husarion/rosbridge:humble-1.3.1-20230220
+    <<: *net-config
+    ports:
+      - 9090:9090
+    environment:
+      - RMW_IMPLEMENTATION=rmw_fastrtps_cpp
+      - FASTRTPS_DEFAULT_PROFILES_FILE=/fastdds.xml 
+    volumes:
+      - ./fastdds.xml:/fastdds.xml
+    command: ros2 launch rosbridge_server rosbridge_websocket_launch.xml
\ No newline at end of file
diff --git a/compose.ros2aria.yaml b/compose.ros2aria.yaml
index 98bfa94..f10100b 100644
--- a/compose.ros2aria.yaml
+++ b/compose.ros2aria.yaml
@@ -1,9 +1,15 @@
 # docker compose -f compose.ros2aria.yaml up
 services:
-  ros2aria:
-   build: .
+  ros2aria-dev:
+   image: delicjusz/pioneer
    network_mode: host
    ipc: host
-   command: ros2 run ros2aria ros2aria --ros-args -p pioneer_id:=${PIONEER_ID}
+   environment:
+    - RMW_IMPLEMENTATION=rmw_fastrtps_cpp
+    - LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/Aria/lib
+    - FASTRTPS_DEFAULT_PROFILES_FILE=/fastdds.xml 
+   volumes:
+    - ./fastdds.xml:/fastdds.xml
    devices:
-           - /dev/ttyS0
+    - /dev/ttyS0
+   command:  ros2 launch pioneer_bringup robot.launch.py pioneer_id:=${PIONEER_ID}
diff --git a/config/rosbot_sensors_foxglove.json b/config/rosbot_sensors_foxglove.json
new file mode 100644
index 0000000..c09bb04
--- /dev/null
+++ b/config/rosbot_sensors_foxglove.json
@@ -0,0 +1,474 @@
+{
+  "configById": {
+    "Plot!gjvhbp": {
+      "paths": [
+        {
+          "value": "/range/fl.range",
+          "enabled": true,
+          "timestampMethod": "receiveTime"
+        },
+        {
+          "value": "/range/rl.range",
+          "enabled": true,
+          "timestampMethod": "receiveTime"
+        },
+        {
+          "value": "/range/fr.range",
+          "enabled": true,
+          "timestampMethod": "receiveTime"
+        },
+        {
+          "value": "/range/rr.range",
+          "enabled": true,
+          "timestampMethod": "receiveTime"
+        }
+      ],
+      "minYValue": -0.039230484541325744,
+      "maxYValue": 1,
+      "showXAxisLabels": true,
+      "showYAxisLabels": true,
+      "showLegend": true,
+      "legendDisplay": "floating",
+      "showPlotValuesInLegend": false,
+      "isSynced": true,
+      "xAxisVal": "timestamp",
+      "sidebarDimension": 240,
+      "foxglovePanelTitle": "Plot",
+      "followingViewWidth": 30
+    },
+    "Plot!1u5bb0v": {
+      "paths": [
+        {
+          "value": "/imu_broadcaster/imu.orientation.w",
+          "enabled": true,
+          "timestampMethod": "receiveTime"
+        },
+        {
+          "value": "/imu_broadcaster/imu.orientation.x",
+          "enabled": true,
+          "timestampMethod": "receiveTime"
+        },
+        {
+          "value": "/imu_broadcaster/imu.orientation.y",
+          "enabled": true,
+          "timestampMethod": "receiveTime"
+        },
+        {
+          "value": "/imu_broadcaster/imu.orientation.z",
+          "enabled": true,
+          "timestampMethod": "receiveTime"
+        }
+      ],
+      "minYValue": -1.1,
+      "maxYValue": 1.1,
+      "showXAxisLabels": true,
+      "showYAxisLabels": true,
+      "showLegend": true,
+      "legendDisplay": "floating",
+      "showPlotValuesInLegend": false,
+      "isSynced": true,
+      "xAxisVal": "timestamp",
+      "sidebarDimension": 240,
+      "foxglovePanelTitle": "Plot",
+      "followingViewWidth": 30
+    },
+    "Tab!2qhku9u": {
+      "activeTabIdx": 0,
+      "tabs": [
+        {
+          "title": "Ranges Plots",
+          "layout": "Plot!gjvhbp"
+        },
+        {
+          "title": "IMU Plots",
+          "layout": "Plot!1u5bb0v"
+        }
+      ]
+    },
+    "3D!40jejke": {
+      "cameraState": {
+        "perspective": true,
+        "distance": 4.7565377051017865,
+        "phi": 0.5121483430698642,
+        "thetaOffset": 89.80364880250498,
+        "targetOffset": [
+          0.04217571585974451,
+          -0.03971452594915587,
+          1.5648103050465003e-17
+        ],
+        "target": [
+          0,
+          0,
+          0
+        ],
+        "targetOrientation": [
+          0,
+          0,
+          0,
+          1
+        ],
+        "fovy": 45,
+        "near": 0.5,
+        "far": 5000
+      },
+      "followMode": "follow-pose",
+      "scene": {
+        "transforms": {
+          "axisScale": 0.4499999999999996,
+          "labelSize": 0.013834613718625963
+        },
+        "enableStats": true,
+        "ignoreColladaUpAxis": false,
+        "syncCamera": false
+      },
+      "transforms": {
+        "frame:base_link": {
+          "visible": false
+        },
+        "frame:body_link": {
+          "visible": false
+        },
+        "frame:cover_link": {
+          "visible": false
+        },
+        "frame:imu_link": {
+          "visible": false
+        },
+        "frame:camera_link": {
+          "visible": false
+        },
+        "frame:fl_range": {
+          "visible": false
+        },
+        "frame:fr_range": {
+          "visible": false
+        },
+        "frame:rl_range": {
+          "visible": false
+        },
+        "frame:rr_range": {
+          "visible": false
+        },
+        "frame:fl_wheel_link": {
+          "visible": true
+        },
+        "frame:fr_wheel_link": {
+          "visible": true
+        },
+        "frame:rl_wheel_link": {
+          "visible": true
+        },
+        "frame:rr_wheel_link": {
+          "visible": true
+        },
+        "frame:slamtec_rplidar_a2_link": {
+          "visible": false
+        },
+        "frame:laser": {
+          "visible": false
+        },
+        "frame:orbbec_astra_link": {
+          "visible": false
+        },
+        "frame:depth": {
+          "visible": false
+        },
+        "frame:odom": {
+          "visible": false
+        }
+      },
+      "topics": {
+        "/scan": {
+          "visible": true,
+          "colorField": "range",
+          "colorMode": "flat",
+          "colorMap": "turbo",
+          "pointSize": 8,
+          "flatColor": "#f90000"
+        },
+        "/robot_description": {
+          "visible": false
+        }
+      },
+      "layers": {
+        "e827a6dc-875b-448a-8475-5497577c2e1b": {
+          "visible": true,
+          "frameLocked": true,
+          "label": "URDF",
+          "instanceId": "e827a6dc-875b-448a-8475-5497577c2e1b",
+          "layerId": "foxglove.Urdf",
+          "url": "http://{{.Host}}:{{env "FOXGLOVE_PORT"}}/rosbot.urdf",
+          "order": 1
+        },
+        "b2c64820-a936-4f65-82b5-7a04ef902009": {
+          "visible": true,
+          "frameLocked": true,
+          "label": "Grid",
+          "instanceId": "b2c64820-a936-4f65-82b5-7a04ef902009",
+          "layerId": "foxglove.Grid",
+          "size": 10,
+          "divisions": 100,
+          "lineWidth": 1,
+          "color": "#248eff",
+          "position": [
+            0,
+            0,
+            0
+          ],
+          "rotation": [
+            0,
+            0,
+            0
+          ],
+          "order": 2
+        }
+      },
+      "publish": {
+        "type": "point",
+        "poseTopic": "/move_base_simple/goal",
+        "pointTopic": "/clicked_point",
+        "poseEstimateTopic": "/initialpose",
+        "poseEstimateXDeviation": 0.5,
+        "poseEstimateYDeviation": 0.5,
+        "poseEstimateThetaDeviation": 0.26179939
+      },
+      "foxglovePanelTitle": "Lidar View"
+    },
+    "RosOut!b0toow": {
+      "searchTerms": [],
+      "minLogLevel": 2
+    },
+    "Teleop!yh7wcv": {
+      "topic": "/cmd_vel",
+      "publishRate": 1,
+      "upButton": {
+        "field": "linear-x",
+        "value": 0.19754204525471783
+      },
+      "downButton": {
+        "field": "linear-x",
+        "value": -0.20000000000000015
+      },
+      "leftButton": {
+        "field": "angular-z",
+        "value": 1
+      },
+      "rightButton": {
+        "field": "angular-z",
+        "value": -1
+      },
+      "foxglovePanelTitle": "Diff Drive"
+    },
+    "ImageViewPanel!40iocf4": {
+      "cameraTopic": "/camera/color/image_raw/compressed",
+      "enabledMarkerTopics": [],
+      "mode": "fit",
+      "pan": {
+        "x": 0,
+        "y": 0
+      },
+      "rotation": 0,
+      "synchronize": true,
+      "transformMarkers": false,
+      "zoom": 1,
+      "foxglovePanelTitle": "Astra Compressed Image"
+    },
+    "Gauge!4jffafa": {
+      "path": "/battery.voltage",
+      "minValue": 9.8,
+      "maxValue": 12.6,
+      "colorMap": "turbo",
+      "colorMode": "colormap",
+      "gradient": [
+        "#0000ff",
+        "#ff00ff"
+      ],
+      "reverse": false,
+      "foxglovePanelTitle": "Battery"
+    },
+    "Indicator!11kizr9": {
+      "path": "/battery.voltage",
+      "style": "background",
+      "fallbackColor": "#000000",
+      "fallbackLabel": "Ok",
+      "rules": [
+        {
+          "operator": "<",
+          "rawValue": "10.8",
+          "color": "#ff0000",
+          "label": "Plug charger!"
+        }
+      ],
+      "foxglovePanelTitle": "Plug Charger Info"
+    },
+    "Plot!4dl4s92": {
+      "paths": [
+        {
+          "value": "/battery.voltage",
+          "enabled": true,
+          "timestampMethod": "receiveTime"
+        }
+      ],
+      "minYValue": 9.482842712474614,
+      "maxYValue": 13,
+      "showXAxisLabels": true,
+      "showYAxisLabels": true,
+      "showLegend": true,
+      "legendDisplay": "floating",
+      "showPlotValuesInLegend": true,
+      "isSynced": true,
+      "xAxisVal": "timestamp",
+      "sidebarDimension": 240,
+      "foxglovePanelTitle": "Plot",
+      "followingViewWidth": 60
+    },
+    "Publish!1f6cruz": {
+      "topicName": "/led/left",
+      "datatype": "std_msgs/msg/Bool",
+      "buttonText": "LED1 Turn ON",
+      "buttonTooltip": "",
+      "buttonColor": "#00A871",
+      "advancedView": false,
+      "value": "{\n  \"data\": true\n}",
+      "foxglovePanelTitle": "LED1 Turn ON"
+    },
+    "Publish!1wozu40": {
+      "topicName": "/led/right",
+      "datatype": "std_msgs/msg/Bool",
+      "buttonText": "LED2 Turn ON",
+      "buttonTooltip": "",
+      "buttonColor": "#00A871",
+      "advancedView": false,
+      "value": "{\n  \"data\": true\n}",
+      "foxglovePanelTitle": "LED2 Turn ON"
+    },
+    "Indicator!2z34jcy": {
+      "path": "/button/left.data",
+      "style": "background",
+      "fallbackColor": "#ff0000",
+      "fallbackLabel": "Released",
+      "rules": [
+        {
+          "operator": "=",
+          "rawValue": "true",
+          "color": "#26c578",
+          "label": "Pressed"
+        }
+      ],
+      "foxglovePanelTitle": "Left Button"
+    },
+    "Publish!3cb71c0": {
+      "topicName": "/led/left",
+      "datatype": "std_msgs/msg/Bool",
+      "buttonText": "LED1 Turn OFF",
+      "buttonTooltip": "",
+      "buttonColor": "#a80000",
+      "advancedView": false,
+      "value": "{\n  \"data\": false\n}",
+      "foxglovePanelTitle": "LED1 Turn OFF"
+    },
+    "Publish!46onm9c": {
+      "topicName": "/led/right",
+      "datatype": "std_msgs/msg/Bool",
+      "buttonText": "LED2 Turn OFF",
+      "buttonTooltip": "",
+      "buttonColor": "#a80000",
+      "advancedView": false,
+      "value": "{\n  \"data\": false\n}",
+      "foxglovePanelTitle": "LED2 Turn OFF"
+    },
+    "Indicator!1hywfa1": {
+      "path": "/button/right.data",
+      "style": "background",
+      "fallbackColor": "#ff0000",
+      "fallbackLabel": "Released",
+      "rules": [
+        {
+          "operator": "=",
+          "rawValue": "true",
+          "color": "#26c578",
+          "label": "Pressed"
+        }
+      ],
+      "foxglovePanelTitle": "Left Button"
+    }
+  },
+  "globalVariables": {
+    "globalVariable": 7
+  },
+  "userNodes": {
+    "f5206e1d-deee-4f90-a03e-f561fbb9a7dd": {
+      "sourceCode": "// The ./types module provides helper types for your Input events and messages.\nimport { Input, Message } from \"./types\";\n\n// Your script can output well-known message types, any of your custom message types, or\n// complete custom message types.\n//\n// Use `Message` to access your data source types or well-known types:\n// type Twist = Message<\"geometry_msgs/Twist\">;\n//\n// Conventionally, it's common to make a _type alias_ for your script's output type\n// and use that type name as the return type for your script function.\n// Here we've called the type `Output` but you can pick any type name.\ntype Output = {\n  hello: string;\n};\n\n// These are the topics your script \"subscribes\" to. Studio will invoke your script function\n// when any message is received on one of these topics.\nexport const inputs = [\"/input/topic\"];\n\n// Any output your script produces is \"published\" to this topic. Published messages are only visible within Studio, not to your original data source.\nexport const output = \"/studio_script/output_topic\";\n\n// This function is called with messages from your input topics.\n// The first argument is an event with the topic, receive time, and message.\n// Use the `Input<...>` helper to get the correct event type for your input topic messages.\nexport default function script(event: Input<\"/input/topic\">): Output {\n  return {\n    hello: \"world!\",\n  };\n};",
+      "name": "f5206e1d"
+    },
+    "1a9e6183-d4b1-47dd-a024-efc14ab90b6b": {
+      "sourceCode": "// This example shows how to subscribe to multiple input topics.\n//\n// NOTE:\n// User Scripts can subscribe to multiple input topics, but can only publish on a single topic.\n\nimport { Input } from \"./types\";\n\ntype Output = { topic: string };\ntype GlobalVariables = { id: number };\n\n// List all the input topics in the `input` array\nexport const inputs = [\"/input/topic\", \"/input/another\"];\nexport const output = \"/studio_script/output_topic\";\n\n// Make an InputEvent type alias. Since our node will get a message from either input topic, we need to enumerate the topics.\ntype InputEvent = Input<\"/input/topic\"> | Input<\"/input/another\">;\n\nexport default function node(event: InputEvent, globalVars: GlobalVariables): Output {\n  // Remember that your node will get messages on each topic, so you\n  // need to check each event's topic to know which fields are available on the message.\n  switch (event.topic) {\n    case \"/input/topic\":\n      // topic specific input logic\n      // Our message fields are specific to our topic message\n      break;\n    case \"/input/another\":\n      // another specific logic\n      break;\n  }\n\n  // Nodes can only output one type of message regardless of the inputs\n  // Here we echo back the input topic as an example.\n  return {\n    topic: event.topic,\n  };\n};\n",
+      "name": "1a9e6183"
+    }
+  },
+  "playbackConfig": {
+    "speed": 1
+  },
+  "layout": {
+    "first": {
+      "direction": "row",
+      "first": "Tab!2qhku9u",
+      "second": {
+        "first": "3D!40jejke",
+        "second": {
+          "first": "RosOut!b0toow",
+          "second": "Teleop!yh7wcv",
+          "direction": "column"
+        },
+        "direction": "row",
+        "splitPercentage": 57.03330110757996
+      },
+      "splitPercentage": 30.5849582172702
+    },
+    "second": {
+      "first": "ImageViewPanel!40iocf4",
+      "second": {
+        "first": {
+          "first": {
+            "first": "Gauge!4jffafa",
+            "second": "Indicator!11kizr9",
+            "direction": "column"
+          },
+          "second": "Plot!4dl4s92",
+          "direction": "row"
+        },
+        "second": {
+          "first": {
+            "first": "Publish!1f6cruz",
+            "second": {
+              "first": "Publish!1wozu40",
+              "second": "Indicator!2z34jcy",
+              "direction": "row",
+              "splitPercentage": 30.461538461538503
+            },
+            "direction": "row",
+            "splitPercentage": 23.167848699763567
+          },
+          "second": {
+            "first": {
+              "first": "Publish!3cb71c0",
+              "second": "Publish!46onm9c",
+              "direction": "row",
+              "splitPercentage": 49.49494949494945
+            },
+            "second": "Indicator!1hywfa1",
+            "direction": "row",
+            "splitPercentage": 46.808510638297854
+          },
+          "direction": "column"
+        },
+        "direction": "row",
+        "splitPercentage": 57.106729218589656
+      },
+      "direction": "row",
+      "splitPercentage": 30.63012301119672
+    },
+    "direction": "column",
+    "splitPercentage": 67.44897959183673
+  }
+}
\ No newline at end of file
diff --git a/fastdds.xml b/fastdds.xml
new file mode 100644
index 0000000..26f783a
--- /dev/null
+++ b/fastdds.xml
@@ -0,0 +1,25 @@
+<dds>
+    <profiles>
+        <participant profile_name="super_client_profile" is_default_profile="true">
+            <rtps>
+                <builtin>
+                    <discovery_config>
+                        <discoveryProtocol>SUPER_CLIENT</discoveryProtocol>
+                        <discoveryServersList>
+                            <RemoteServer prefix="44.53.00.5f.45.50.52.4f.53.49.4d.41">
+                                <metatrafficUnicastLocatorList>
+                                    <locator>
+                                        <udpv4>
+                                            <address>10.104.16.240</address>
+                                            <port>11811</port>
+                                        </udpv4>
+                                    </locator>
+                                </metatrafficUnicastLocatorList>
+                            </RemoteServer>
+                        </discoveryServersList>
+                    </discovery_config>
+                </builtin>
+            </rtps>
+        </participant>
+    </profiles>
+</dds>
diff --git a/pioneer3dx.urdf b/pioneer3dx.urdf
new file mode 100644
index 0000000..a39929b
--- /dev/null
+++ b/pioneer3dx.urdf
@@ -0,0 +1,310 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from src/pioneer_description/urdf/pioneer3dx.urdf.xacro | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="pioneer3dx" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor">
+  <!--<include filename="$(find amr_robots_description)/urdf/materials.xacro"/>-->
+  <!-- Chassis -->
+  <link name="base_link">
+    <inertial>
+      <mass value="3.5"/>
+      <!--<origin xyz="-0.025 0 -0.223"/>-->
+      <origin xyz="0 0 0"/>
+      <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="-0.045 0 0.148"/>
+      <geometry name="pioneer_geom">
+        <mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/meshes/p3dx_meshes/chassis.stl"/>
+      </geometry>
+      <material name="ChassisRed">
+        <color rgba="0.851 0.0 0.0 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="top_plate">
+    <inertial>
+      <mass value="0.01"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry name="pioneer_geom">
+        <mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/meshes/p3dx_meshes/top.stl"/>
+      </geometry>
+      <material name="TopBlack">
+        <color rgba="0.038 0.038 0.038 1.0"/>
+      </material>
+    </visual>
+  </link>
+  <gazebo reference="top_plate">
+    <!-- material value="Gazebo/Black"/ -->
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="base_top_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="-0.045 0 0.234"/>
+    <axis xzy="0 0 1"/>
+    <parent link="base_link"/>
+    <child link="top_plate"/>
+  </joint>
+  <link name="front_sonar">
+    <inertial>
+      <mass value="0.0001"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry name="pioneer_geom">
+        <mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/meshes/p3dx_meshes/front_sonar.stl"/>
+      </geometry>
+      <material name="SonarYellow">
+        <color rgba="0.715 0.583 0.210 1.0"/>
+      </material>
+    </visual>
+  </link>
+  <gazebo reference="front_sonar">
+    <material value="Gazebo/Yellow"/>
+  </gazebo>
+  <joint name="base_front_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="-0.198 0 0.208"/>
+    <parent link="base_link"/>
+    <child link="front_sonar"/>
+  </joint>
+  <joint name="base_back_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0.109 0 0.209"/>
+    <parent link="base_link"/>
+    <child link="back_sonar"/>
+  </joint>
+  <link name="back_sonar">
+    <inertial>
+      <mass value="0"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry name="pioneer_geom">
+        <mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/meshes/p3dx_meshes/back_sonar.stl"/>
+      </geometry>
+      <material name="SonarYellow">
+        <color rgba="0.715 0.583 0.210 1.0"/>
+      </material>
+    </visual>
+  </link>
+  <!-- Caster -->
+  <joint name="base_caster_swivel_joint" type="continuous">
+    <origin rpy="0 0 0" xyz="-0.185 0 0.055"/>
+    <anchor xyz="0 0 0"/>
+    <limit effort="100" k_velocity="0" velocity="100"/>
+    <joint_properties damping="0.0" friction="0.0"/>
+    <parent link="base_link"/>
+    <child link="caster_swivel"/>
+  </joint>
+  <link name="caster_swivel">
+    <inertial>
+      <mass value="0.1"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry name="pioneer_geom">
+        <mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/meshes/p3dx_meshes/caster_swivel.stl"/>
+      </geometry>
+      <material name="caster_swivel">
+        <color rgba="0.5 0.5 0.5 1"/>
+      </material>
+    </visual>
+  </link>
+  <gazebo reference="caster_swivel">
+    <material value="Gazebo/Grey"/>
+  </gazebo>
+  <!-- Center Wheel + Hubcap -->
+  <link name="caster_hubcap">
+    <inertial>
+      <mass value="0.01"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry name="pioneer_geom">
+        <mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/meshes/p3dx_meshes/caster_hubcap.stl"/>
+      </geometry>
+      <material name="caster_swivel">
+        <color rgba="0.5 0.5 0.5 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.05 0.05 0.05"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="caster_hubcap">
+    <material value="Gazebo/Grey"/>
+  </gazebo>
+  <joint name="caster_swivel_hubcap_joint" type="continuous">
+    <origin rpy="0 0 0" xyz="-0.026 0 -0.016"/>
+    <axis xyz="0 1 0"/>
+    <anchor xyz="0 0 0"/>
+    <limit effort="100" k_velocity="0" velocity="100"/>
+    <joint_properties damping="0.0" friction="0.0"/>
+    <parent link="caster_swivel"/>
+    <child link="caster_wheel"/>
+  </joint>
+  <link name="caster_wheel">
+    <inertial>
+      <mass value="0.1"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry name="pioneer_geom">
+        <mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/meshes/p3dx_meshes/caster_wheel.stl"/>
+      </geometry>
+      <material name="WheelBlack">
+        <color rgba="0.117 0.117 0.117 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="-1.57079635 0 0" xyz="0 0 0"/>
+      <geometry>
+        <!--<mesh filename="$(find pioneer_description)/meshes/meshes/p3dx_meshes/caster_wheel.stl"/>-->
+        <cylinder length="0.01" radius="0.0375"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="left_hub">
+    <material value="Gazebo/Yellow"/>
+  </gazebo>
+  <joint name="caster_wheel_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="-0.0035 0 -0.001"/>
+    <parent link="caster_wheel"/>
+    <child link="caster_hubcap"/>
+  </joint>
+  <link name="left_hub">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/meshes/p3dx_meshes/left_hubcap.stl"/>
+      </geometry>
+      <material name="HubcapYellow">
+        <color rgba="1.0 0.811 0.151 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="-1.57079635 0 0" xyz="0 0 0"/>
+      <geometry>
+        <!--<mesh filename="$(find pioneer_description)/meshes/meshes/p3dx_meshes/caster_wheel.stl"/>-->
+        <cylinder length="0.01" radius="0.09"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_hub_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0.15 0.08"/>
+    <parent link="base_link"/>
+    <child link="left_hub"/>
+    <axis xyz="0 1 0"/>
+  </joint>
+  <link name="left_wheel">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/meshes/p3dx_meshes/left_wheel.stl"/>
+      </geometry>
+      <material name="WheelBlack">
+        <color rgba="0.117 0.117 0.117 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="left_wheel_joint" type="fixed">
+    <!-- type="continuous" -->
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="left_hub"/>
+    <child link="left_wheel"/>
+  </joint>
+  <gazebo reference="left_wheel_joint">
+    <material value="Gazebo/Black"/>
+  </gazebo>
+  <link name="right_hub">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/meshes/p3dx_meshes/right_hubcap.stl"/>
+      </geometry>
+      <material name="HubcapYellow">
+        <color rgba="1.0 0.811 0.151 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="-1.57079635 0 0" xyz="0 0 0"/>
+      <geometry>
+        <!--<mesh filename="$(find pioneer_description)/meshes/meshes/p3dx_meshes/caster_wheel.stl"/>-->
+        <cylinder length="0.01" radius="0.09"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="right_hub">
+    <material value="Gazebo/Yellow"/>
+  </gazebo>
+  <joint name="right_hub_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 -0.15 0.08"/>
+    <parent link="base_link"/>
+    <child link="right_hub"/>
+    <axis xyz="0 1 0"/>
+  </joint>
+  <link name="right_wheel">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="/home/jdelicat/lab_repos/ros2aria/install/pioneer_description/share/pioneer_description/meshes/meshes/p3dx_meshes/right_wheel.stl"/>
+      </geometry>
+      <material name="WheelBlack">
+        <color rgba="0.117 0.117 0.117 1"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="right_wheel_joint" type="fixed">
+    <!-- type="continuous" -->
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="right_hub"/>
+    <child link="right_wheel"/>
+  </joint>
+  <gazebo reference="right_wheel_joint">
+    <material value="Gazebo/Black"/>
+  </gazebo>
+  <create>
+    <back_sonar parent="base_link"/>
+    <top_plate parent="base_link"/>
+  </create>
+  <gazebo>
+    <plugin filename="libgazebo_ros_diff_drive.so" name="differential_drive_controller">
+      <alwaysOn>true</alwaysOn>
+      <updateRate>100</updateRate>
+      <leftJoint>left_wheel_joint</leftJoint>
+      <rightJoint>right_wheel_joint</rightJoint>
+      <wheelSeparation>0.158</wheelSeparation>
+      <wheelDiameter>0.12</wheelDiameter>
+      <torque>5</torque>
+      <!-- interface:position name="position_iface_0"/ -->
+      <commandTopic>cmd_vel</commandTopic>
+      <robotBaseFrame>base_link</robotBaseFrame>
+      <odometryTopic>odom</odometryTopic>
+      <odometryFrame>odom</odometryFrame>
+    </plugin>
+    <!-- TODO include P3D (ground truth) plugin -->
+    <!-- XXX old urdf included a gazebo_ros_controller_manager plugin with a 1 second update rate -->
+  </gazebo>
+</robot>
diff --git a/run.sh b/run.sh
index a9917bf..a8e9729 100755
--- a/run.sh
+++ b/run.sh
@@ -1,6 +1,6 @@
 #!/bin/bash
 export LD_LIBRARY_PATH=/usr/local/Aria/lib
 source /opt/ros/foxy/setup.bash
-source /ws/ros2aria/install/setup.bash
+source /ros2_ws/ros2aria/install/setup.bash
 ros2 run ros2aria ros2aria
 
diff --git a/src/pioneer_bringup/CMakeLists.txt b/src/pioneer_bringup/CMakeLists.txt
new file mode 100644
index 0000000..3dcebe2
--- /dev/null
+++ b/src/pioneer_bringup/CMakeLists.txt
@@ -0,0 +1,11 @@
+cmake_minimum_required(VERSION 3.8)
+project(pioneer_bringup)
+
+find_package(ament_cmake REQUIRED)
+
+install(DIRECTORY 
+  launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()
diff --git a/src/pioneer_description/launch/pioneer_description.launch.py b/src/pioneer_bringup/launch/robot.launch.py
similarity index 52%
rename from src/pioneer_description/launch/pioneer_description.launch.py
rename to src/pioneer_bringup/launch/robot.launch.py
index cf6fd8e..6953eb7 100644
--- a/src/pioneer_description/launch/pioneer_description.launch.py
+++ b/src/pioneer_bringup/launch/robot.launch.py
@@ -13,25 +13,38 @@ from launch.substitutions import LaunchConfiguration
 from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.conditions import IfCondition
+from launch_ros.actions import Node, PushRosNamespace
+from ament_index_python.packages import get_package_share_directory
 
 
 def generate_launch_description():
-    pkg_share = launch_ros.substitutions.FindPackageShare(
-        package='pioneer_description').find('micromouse_description')
+    pioneer_description = get_package_share_directory('pioneer_description')
+    default_model_path = os.path.join(pioneer_description, 'urdf/pioneer3dx.urdf.xacro')
 
-    default_model_path = os.path.join(
-        pkg_share, 'urdf/pioneer3dx.urdf.xacro')
-
-    robot_state_publisher_node = launch_ros.actions.Node(
-        namespace='pioneer5',
-        package='robot_state_publisher',
-        executable='robot_state_publisher',
-        parameters=[{'robot_description': Command(
-            ['xacro ', LaunchConfiguration('model')])}]
+    namespace = LaunchConfiguration('namespace')
+    namespace_arg = DeclareLaunchArgument(
+        'namespace',
+        default_value='pioneer0/'
     )
 
-    return launch.LaunchDescription([
-        launch.actions.DeclareLaunchArgument(name='model', default_value=default_model_path,
-                                             description='Absolute path to robot urdf file'),
+    robot_state_publisher_node = Node(
+        namespace=namespace,
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        parameters=[
+            {'robot_description': Command(['xacro ', default_model_path])},
+            {'frame_prefix': namespace}
+        ]
+    )
+
+    ros2aria = Node(
+        namespace=namespace,
+        package='ros2aria',
+        executable='ros2aria',
+    )
+
+    return LaunchDescription([
+        namespace_arg,
         robot_state_publisher_node,
+        ros2aria
     ])
diff --git a/src/pioneer_bringup/package.xml b/src/pioneer_bringup/package.xml
new file mode 100644
index 0000000..357174e
--- /dev/null
+++ b/src/pioneer_bringup/package.xml
@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>pioneer_bringup</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="jakub.delicat@husarion.com">deli</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>xacro</depend>
+  <depend>joint_state_publisher</depend>
+  <depend>tf2_ros</depend>
+
+  <depend>pioneer_description</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/src/pioneer_description/CMakeLists.txt b/src/pioneer_description/CMakeLists.txt
index cdbbc4b..55a633e 100644
--- a/src/pioneer_description/CMakeLists.txt
+++ b/src/pioneer_description/CMakeLists.txt
@@ -1,30 +1,14 @@
 cmake_minimum_required(VERSION 3.8)
 project(pioneer_description)
 
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-# find dependencies
 find_package(ament_cmake REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
 
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # comment the line when a copyright and license is added to all source files
-  set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # comment the line when this package is in a git repo and when
-  # a copyright and license is added to all source files
-  set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
-endif()
-
-install(DIRECTORY urdf launch
+install(
+  DIRECTORY
+  meshes
+  urdf
   DESTINATION share/${PROJECT_NAME}
 )
 
-ament_package()
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()
\ No newline at end of file
diff --git a/src/pioneer_description/meshes/back_rim.stl b/src/pioneer_description/meshes/back_rim.stl
new file mode 100644
index 0000000..09587a5
Binary files /dev/null and b/src/pioneer_description/meshes/back_rim.stl differ
diff --git a/src/pioneer_description/meshes/caster_hubcap.stl b/src/pioneer_description/meshes/caster_hubcap.stl
new file mode 100644
index 0000000..c9400f7
Binary files /dev/null and b/src/pioneer_description/meshes/caster_hubcap.stl differ
diff --git a/src/pioneer_description/meshes/caster_swivel.stl b/src/pioneer_description/meshes/caster_swivel.stl
new file mode 100644
index 0000000..03797a2
Binary files /dev/null and b/src/pioneer_description/meshes/caster_swivel.stl differ
diff --git a/src/pioneer_description/meshes/caster_wheel.stl b/src/pioneer_description/meshes/caster_wheel.stl
new file mode 100644
index 0000000..c8b857d
Binary files /dev/null and b/src/pioneer_description/meshes/caster_wheel.stl differ
diff --git a/src/pioneer_description/meshes/chassis.stl b/src/pioneer_description/meshes/chassis.stl
new file mode 100644
index 0000000..13aa2cf
Binary files /dev/null and b/src/pioneer_description/meshes/chassis.stl differ
diff --git a/src/pioneer_description/meshes/front_rim.stl b/src/pioneer_description/meshes/front_rim.stl
new file mode 100644
index 0000000..c3074d5
Binary files /dev/null and b/src/pioneer_description/meshes/front_rim.stl differ
diff --git a/src/pioneer_description/meshes/front_sonar.stl b/src/pioneer_description/meshes/front_sonar.stl
new file mode 100644
index 0000000..82b9ce0
Binary files /dev/null and b/src/pioneer_description/meshes/front_sonar.stl differ
diff --git a/src/pioneer_description/meshes/hokuyo_convex.stl b/src/pioneer_description/meshes/hokuyo_convex.stl
new file mode 100644
index 0000000..80e7b91
Binary files /dev/null and b/src/pioneer_description/meshes/hokuyo_convex.stl differ
diff --git a/src/pioneer_description/meshes/left_hubcap.stl b/src/pioneer_description/meshes/left_hubcap.stl
new file mode 100644
index 0000000..01bd2f9
Binary files /dev/null and b/src/pioneer_description/meshes/left_hubcap.stl differ
diff --git a/src/pioneer_description/meshes/left_wheel.stl b/src/pioneer_description/meshes/left_wheel.stl
new file mode 100644
index 0000000..31137e2
Binary files /dev/null and b/src/pioneer_description/meshes/left_wheel.stl differ
diff --git a/src/pioneer_description/meshes/right_hubcap.stl b/src/pioneer_description/meshes/right_hubcap.stl
new file mode 100644
index 0000000..ac3f36f
Binary files /dev/null and b/src/pioneer_description/meshes/right_hubcap.stl differ
diff --git a/src/pioneer_description/meshes/right_wheel.stl b/src/pioneer_description/meshes/right_wheel.stl
new file mode 100644
index 0000000..c518c0e
Binary files /dev/null and b/src/pioneer_description/meshes/right_wheel.stl differ
diff --git a/src/pioneer_description/meshes/sonar_frame.stl b/src/pioneer_description/meshes/sonar_frame.stl
new file mode 100644
index 0000000..9153bc5
Binary files /dev/null and b/src/pioneer_description/meshes/sonar_frame.stl differ
diff --git a/src/pioneer_description/meshes/top.stl b/src/pioneer_description/meshes/top.stl
new file mode 100644
index 0000000..302ed5f
Binary files /dev/null and b/src/pioneer_description/meshes/top.stl differ
diff --git a/src/pioneer_description/package.xml b/src/pioneer_description/package.xml
index 1e31bf7..30dad0c 100644
--- a/src/pioneer_description/package.xml
+++ b/src/pioneer_description/package.xml
@@ -1,18 +1,32 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
+<package>
   <name>pioneer_description</name>
-  <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="jakub.delicat@husarion.com">deli</maintainer>
-  <license>TODO: License declaration</license>
+  <version>1.1.0</version>
+  <description>URDF file descriptions for various Adept MobileRobots/ActivMedia robots</description>
+  <maintainer email="reed.hedges@adept.com">Reed Hedges</maintainer> 
 
-  <buildtool_depend>ament_cmake</buildtool_depend>
+  <license>BSD</license>
 
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
+  <url type="website">http://wiki.ros.org/Robots/Pioneer</url>
 
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>
+  <author email="hunter.allen@Vanderbilt.edu">Hunter Allen</author>
+  <author email="dfseifer@usc.edu">David Feil-Seifer</author>
+  <author email="reed.hedges@adept.com">Reed Hedges</author>
+  <author>Brian Gerkey</author>
+  <author>Kasper Stoy</author>
+  <author>Richard Vaughan</author>
+  <author>Andrew Howard</author>
+  <author>Tucker Hermans</author>
+  <author>ActivMedia Robotics LLC</author>
+  <author>MobileRobots Inc.</author>
+  <author email="jakub.delicat@pwr.edu.pl">Jakub Delicat</author>
+
+  <build_depend>urdf</build_depend>
+  <build_depend>robot_state_publisher</build_depend>
+
+  <run_depend>urdf</run_depend>
+  <run_depend>robot_state_publisher</run_depend>
+
+  <export>                               
+   <build_type>ament_cmake</build_type> 
+  </export> 
+</package>
\ No newline at end of file
diff --git a/src/pioneer_description/urdf/pioneer3dx.urdf b/src/pioneer_description/urdf/pioneer3dx.urdf
deleted file mode 100644
index a6e0a7d..0000000
--- a/src/pioneer_description/urdf/pioneer3dx.urdf
+++ /dev/null
@@ -1,367 +0,0 @@
-<?xml version="1.0"?>
-<!-- =================================================================================== -->
-<!-- |    This document was autogenerated by xacro from pioneer3dx.urdf.xacro          | -->
-<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
-<!-- =================================================================================== -->
-<robot name="pioneer3dx" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://ros.org/wiki/xacro">
-    <!--<include filename="$(find amr_robots_description)/urdf/materials.xacro"/>-->
-    <!-- Chassis -->
-
-    <link name="base_link">
-        <inertial>
-            <mass value="3.5" />
-            <!--<origin xyz="-0.025 0 -0.223"/>-->
-            <origin xyz="0 0 0" />
-            <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
-        </inertial>
-
-        <visual>
-            <origin rpy="0 0 0" xyz="-0.045 0 0.148" />
-            <geometry name="pioneer_geom">
-                <mesh filename="package://amr_robots_description/meshes/p3dx_meshes/chassis.stl" />
-            </geometry>
-            <material name="ChassisRed">
-                <color rgba="0.851 0.0 0.0 1.0" />
-            </material>
-        </visual>
-        <collision>
-            <origin rpy="0 0 0" xyz="0 0 0" />
-            <geometry>
-                <box size="0.01 0.01 0.01" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="top_plate">
-        <inertial>
-            <mass value="0.01" />
-            <origin xyz="0 0 0" />
-            <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
-        </inertial>
-        <visual>
-            <origin rpy="0 0 0" xyz="0 0 0" />
-            <geometry name="pioneer_geom">
-                <mesh filename="package://amr_robots_description/meshes/p3dx_meshes/top.stl" />
-            </geometry>
-            <material name="TopBlack">
-                <color rgba="0.038 0.038 0.038 1.0" />
-            </material>
-        </visual>
-    </link>
-    <gazebo reference="top_plate">
-        <!-- material value="Gazebo/Black"/ -->
-        <material>Gazebo/Black</material>
-    </gazebo>
-    <joint name="base_top_joint" type="fixed">
-        <origin rpy="0 0 0" xyz="-0.045 0 0.234" />
-        <axis xzy="0 0 1" />
-        <parent link="base_link" />
-        <child link="top_plate" />
-    </joint>
-    <!-- <link name="front_sonar">
-    <inertial>
-      <mass value="0.0001"/>
-      <origin xyz="0 0 0"/>
-      <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry name="pioneer_geom">
-        <mesh filename="package://amr_robots_description/meshes/p3dx_meshes/front_sonar.stl"/>
-      </geometry>
-      <material name="SonarYellow">
-        <color rgba="0.715 0.583 0.210 1.0"/>
-      </material>
-    </visual>
-  </link> -->
-    <!-- <gazebo reference="front_sonar">
-    <material value="Gazebo/Yellow"/>
-  </gazebo> -->
-    <!-- <joint name="base_front_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="-0.198 0 0.208"/>
-    <parent link="base_link"/>
-    <child link="front_sonar"/>
-  </joint> -->
-    <joint name="base_back_joint" type="fixed">
-        <origin rpy="0 0 0" xyz="0.109 0 0.209" />
-        <parent link="base_link" />
-        <child link="sonar_frame" />
-    </joint>
-    <link name="sonar_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" />
-            <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
-        </inertial>
-        <visual>
-            <origin rpy="0 0 0" xyz="0 0 0" />
-            <geometry name="pioneer_geom">
-                <mesh filename="package://amr_robots_description/meshes/p3dx_meshes/sonar_frame.stl" />
-            </geometry>
-            <material name="SonarYellow">
-                <color rgba="0.715 0.583 0.210 1.0" />
-            </material>
-        </visual>
-    </link>
-    <!-- Caster -->
-    <joint name="base_caster_swivel_joint" type="continuous">
-        <origin rpy="0 0 0" xyz="-0.185 0 0.055" />
-        <anchor xyz="0 0 0" />
-        <limit effort="100" k_velocity="0" velocity="100" />
-        <joint_properties damping="0.0" friction="0.0" />
-        <parent link="base_link" />
-        <child link="caster_swivel" />
-    </joint>
-    <link name="caster_swivel">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin rpy="0 0 0" xyz="0 0 0" />
-            <geometry name="pioneer_geom">
-                <mesh filename="package://amr_robots_description/meshes/p3dx_meshes/caster_swivel.stl" />
-            </geometry>
-            <material name="caster_swivel">
-                <color rgba="0.5 0.5 0.5 1" />
-            </material>
-        </visual>
-    </link>
-    <gazebo reference="caster_swivel">
-        <material value="Gazebo/Grey" />
-    </gazebo>
-    <!-- Center Wheel + Hubcap -->
-    <link name="caster_hubcap">
-        <inertial>
-            <mass value="0.01" />
-            <origin xyz="0 0 0" />
-            <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943" />
-        </inertial>
-        <visual>
-            <origin rpy="0 0 0" xyz="0 0 0" />
-            <geometry name="pioneer_geom">
-                <mesh filename="package://amr_robots_description/meshes/p3dx_meshes/caster_hubcap.stl" />
-            </geometry>
-            <material name="caster_swivel">
-                <color rgba="0.5 0.5 0.5 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin rpy="0 0 0" xyz="0 0 0" />
-            <geometry>
-                <box size="0.05 0.05 0.05" />
-            </geometry>
-        </collision>
-    </link>
-    <gazebo reference="caster_hubcap">
-        <material value="Gazebo/Grey" />
-    </gazebo>
-    <joint name="caster_swivel_hubcap_joint" type="continuous">
-        <origin rpy="0 0 0" xyz="-0.026 0 -0.016" />
-        <axis xyz="0 1 0" />
-        <anchor xyz="0 0 0" />
-        <limit effort="100" k_velocity="0" velocity="100" />
-        <joint_properties damping="0.0" friction="0.0" />
-        <parent link="caster_swivel" />
-        <child link="caster_wheel" />
-    </joint>
-    <link name="caster_wheel">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" />
-            <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943" />
-        </inertial>
-        <visual>
-            <origin rpy="0 0 0" xyz="0 0 0" />
-            <geometry name="pioneer_geom">
-                <mesh filename="package://amr_robots_description/meshes/p3dx_meshes/caster_wheel.stl" />
-            </geometry>
-            <material name="WheelBlack">
-                <color rgba="0.117 0.117 0.117 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin rpy="-1.57079635 0 0" xyz="0 0 0" />
-            <geometry>
-                <!--<mesh filename="package://amr_robots_description/meshes/p3dx_meshes/caster_wheel.stl"/>-->
-                <cylinder length="0.01" radius="0.0375" />
-            </geometry>
-        </collision>
-    </link>
-    <gazebo reference="left_hub">
-        <material value="Gazebo/Yellow" />
-    </gazebo>
-    <joint name="caster_wheel_joint" type="fixed">
-        <origin rpy="0 0 0" xyz="-0.0035 0 -0.001" />
-        <parent link="caster_wheel" />
-        <child link="caster_hubcap" />
-    </joint>
-    <link name="left_hub">
-        <visual>
-            <origin rpy="0 0 0" xyz="0 0 0" />
-            <geometry>
-                <mesh filename="package://amr_robots_description/meshes/p3dx_meshes/left_hubcap.stl" />
-            </geometry>
-            <material name="HubcapYellow">
-                <color rgba="1.0 0.811 0.151 1.0" />
-            </material>
-        </visual>
-        <collision>
-            <origin rpy="-1.57079635 0 0" xyz="0 0 0" />
-            <geometry>
-                <!--<mesh filename="package://amr_robots_description/meshes/p3dx_meshes/caster_wheel.stl"/>-->
-                <cylinder length="0.01" radius="0.09" />
-            </geometry>
-        </collision>
-    </link>
-    <joint name="left_hub_joint" type="fixed">
-        <origin rpy="0 0 0" xyz="0 0.15 0.08" />
-        <parent link="base_link" />
-        <child link="left_hub" />
-        <axis xyz="0 1 0" />
-    </joint>
-    <link name="left_wheel">
-        <visual>
-            <origin rpy="0 0 0" xyz="0 0 0" />
-            <geometry>
-                <mesh filename="package://amr_robots_description/meshes/p3dx_meshes/left_wheel.stl" />
-            </geometry>
-            <material name="WheelBlack">
-                <color rgba="0.117 0.117 0.117 1" />
-            </material>
-        </visual>
-    </link>
-    <joint name="left_wheel_joint" type="fixed">
-        <!-- type="continuous" -->
-        <origin rpy="0 0 0" xyz="0 0 0" />
-        <parent link="left_hub" />
-        <child link="left_wheel" />
-    </joint>
-    <gazebo reference="left_wheel_joint">
-        <material value="Gazebo/Black" />
-    </gazebo>
-    <link name="right_hub">
-        <visual>
-            <origin rpy="0 0 0" xyz="0 0 0" />
-            <geometry>
-                <mesh filename="package://amr_robots_description/meshes/p3dx_meshes/right_hubcap.stl" />
-            </geometry>
-            <material name="HubcapYellow">
-                <color rgba="1.0 0.811 0.151 1.0" />
-            </material>
-        </visual>
-        <collision>
-            <origin rpy="-1.57079635 0 0" xyz="0 0 0" />
-            <geometry>
-                <!--<mesh filename="package://amr_robots_description/meshes/p3dx_meshes/caster_wheel.stl"/>-->
-                <cylinder length="0.01" radius="0.09" />
-            </geometry>
-        </collision>
-    </link>
-    <gazebo reference="right_hub">
-        <material value="Gazebo/Yellow" />
-    </gazebo>
-    <joint name="right_hub_joint" type="fixed">
-        <origin rpy="0 0 0" xyz="0 -0.15 0.08" />
-        <parent link="base_link" />
-        <child link="right_hub" />
-        <axis xyz="0 1 0" />
-    </joint>
-    <link name="right_wheel">
-        <visual>
-            <origin rpy="0 0 0" xyz="0 0 0" />
-            <geometry>
-                <mesh filename="package://amr_robots_description/meshes/p3dx_meshes/right_wheel.stl" />
-            </geometry>
-            <material name="WheelBlack">
-                <color rgba="0.117 0.117 0.117 1" />
-            </material>
-        </visual>
-    </link>
-    <joint name="right_wheel_joint" type="fixed">
-        <!-- type="continuous" -->
-        <origin rpy="0 0 0" xyz="0 0 0" />
-        <parent link="right_hub" />
-        <child link="right_wheel" />
-    </joint>
-    <gazebo reference="right_wheel_joint">
-        <material value="Gazebo/Black" />
-    </gazebo>
-    <create>
-        <sonar_frame parent="base_link" />
-        <top_plate parent="base_link" />
-    </create>
-
-
-    <link name="laser">
-        <collision>
-            <origin xyz="0 0 0" rpy="0 0 0" />
-            <geometry>
-                <box size="0.1 0.1 0.1" />
-            </geometry>
-        </collision>
-        <visual>
-            <origin xyz="0 0 0" rpy="0 0 0" />
-            <geometry>
-                <mesh filename="package://amr_robots_description/meshes/p3dx_meshes/hokuyo_convex.stl" />
-            </geometry>
-            <material name="Hokuyo">
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <inertial>
-            <mass value="1e-5" />
-            <origin xyz="0 0 0" rpy="0 0 0" />
-            <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
-        </inertial>
-    </link>
-    <joint name="hokuyo_joint" type="fixed">
-        <axis xyz="0 0 0" />
-        <origin xyz="0.040 0 0.0625" rpy="0 0 0" />
-        <parent link="sonar_frame" />
-        <child link="laser" />
-    </joint>
-
-
-    <link name="camera_frame">
-        <collision>
-            <origin xyz="0 0 0" rpy="0 0 0" />
-            <geometry>
-                <box size="0.095 0.03 0.03" />
-            </geometry>
-        </collision>
-
-    </link>
-    <joint name="camera_joint" type="fixed">
-        <axis xyz="0 0 0" />
-        <origin xyz="0.040 0 0.1125" rpy="0 0 0" />
-        <parent link="sonar_frame" />
-        <child link="camera_frame" />
-    </joint>
-
-
-    <!-- ====================[Gripper]====================== -->
-
-    <!-- <link name="gripper_base">
-  </link> -->
-
-
-    <gazebo>
-        <plugin filename="libgazebo_ros_diff_drive.so" name="differential_drive_controller">
-            <alwaysOn>true</alwaysOn>
-            <updateRate>100</updateRate>
-            <leftJoint>left_wheel_joint</leftJoint>
-            <rightJoint>right_wheel_joint</rightJoint>
-            <wheelSeparation>0.158</wheelSeparation>
-            <wheelDiameter>0.12</wheelDiameter>
-            <torque>5</torque>
-            <!-- interface:position name="position_iface_0"/ -->
-            <commandTopic>cmd_vel</commandTopic>
-            <robotBaseFrame>base_link</robotBaseFrame>
-            <odometryTopic>odom</odometryTopic>
-            <odometryFrame>odom</odometryFrame>
-        </plugin>
-        <!-- TODO include P3D (ground truth) plugin -->
-        <!-- XXX old urdf included a gazebo_ros_controller_manager plugin with a 1 second update rate -->
-    </gazebo>
-</robot>
\ No newline at end of file
diff --git a/src/pioneer_description/urdf/pioneer3dx.urdf.xacro b/src/pioneer_description/urdf/pioneer3dx.urdf.xacro
new file mode 100644
index 0000000..d0675ff
--- /dev/null
+++ b/src/pioneer_description/urdf/pioneer3dx.urdf.xacro
@@ -0,0 +1,350 @@
+<?xml version="1.0"?>
+
+<robot name="pioneer3dx"
+	xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+	xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+	xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+	xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <!--<include filename="$(find amr_robots_description)/urdf/materials.xacro"/>-->
+  <xacro:include filename="$(find pioneer_description)/urdf/pioneer3dx_wheel.xacro"/>
+
+<!-- Chassis -->
+	<link name="base_link">
+		<inertial>
+			<mass value="3.5"/>
+			<!--<origin xyz="-0.025 0 -0.223"/>-->
+			<origin xyz="0 0 0"/>
+			<inertia ixx="1" ixy="0" ixz="0"
+				 iyy="1" iyz="0"
+				 izz="1"/>
+		</inertial>
+		<visual>
+			<origin xyz="-0.045 0 0.148" rpy="0 0 0"/>
+			<geometry name="pioneer_geom">
+				<mesh filename="$(find pioneer_description)/meshes/chassis.stl"/>
+			</geometry>
+			<material name="ChassisRed">
+				<color rgba="0.851 0.0 0.0 1.0"/>
+			</material>
+		</visual>
+		<collision>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+				<geometry>
+					<box size="0.01 0.01 0.01"/>
+				</geometry>
+		</collision>
+	</link>
+
+	<link name="top_plate">
+		<inertial>
+			<mass value="0.01"/>	
+			<origin xyz="0 0 0"/>
+			<inertia ixx="1" ixy="0" ixz="0"
+				 iyy="1" iyz="0"
+ 				izz="1"/>
+		</inertial>
+		<visual>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<geometry name="pioneer_geom">
+				<mesh filename="$(find pioneer_description)/meshes/top.stl"/>
+			</geometry>
+			<material name="TopBlack">
+					<color rgba="0.038 0.038 0.038 1.0"/>
+			</material>
+		</visual>
+	</link>
+	<gazebo reference="top_plate">
+		<!-- material value="Gazebo/Black"/ -->
+		<material>Gazebo/Black</material>
+	</gazebo>
+
+    <joint name="base_top_joint" type="fixed">
+		<origin xyz="-0.045 0 0.234" rpy="0 0 0"/>
+		<axis xzy="0 0 1"/>
+		<parent link="base_link"/>
+		<child link="top_plate"/>
+	</joint>
+
+	<link name="front_sonar">
+		<inertial>
+			<mass value="0.0001"/>	
+			<origin xyz="0 0 0"/>
+			<inertia ixx="1" ixy="0" ixz="0"
+				 iyy="1" iyz="0" izz="1"/>
+		</inertial>
+		<visual>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<geometry name="pioneer_geom">
+				<mesh filename="$(find pioneer_description)/meshes/front_sonar.stl"/>
+			</geometry>
+			<material name="SonarYellow">
+				<color rgba="0.715 0.583 0.210 1.0"/>
+			</material>
+		</visual>
+	</link>
+
+	<gazebo reference="front_sonar">
+		<material value="Gazebo/Yellow"/>
+	</gazebo>
+
+	<joint name="base_front_joint" type="fixed">
+		<origin xyz="-0.198 0 0.208" rpy="0 0 0"/>
+		<parent link="base_link"/>
+		<child link="front_sonar"/>
+	</joint>
+
+	<joint name="base_back_joint" type="fixed">
+		<origin xyz="0.109 0 0.209" rpy="0 0 0"/>
+		<parent link="base_link"/>
+		<child link="back_sonar"/>
+	</joint>
+	<link name="back_sonar">
+		<inertial>
+			<mass value="0"/>	
+			<origin xyz="0 0 0"/>
+			<inertia ixx="1" ixy="0" ixz="0"
+				 iyy="1" iyz="0" izz="1"/>
+		</inertial>
+		<visual>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<geometry name="pioneer_geom">
+				<mesh filename="$(find pioneer_description)/meshes/back_sonar.stl"/>
+			</geometry>
+			<material name="SonarYellow">
+				<color rgba="0.715 0.583 0.210 1.0"/>
+			</material>
+		</visual>
+	</link>
+
+	<!-- Caster -->
+	<joint name="base_caster_swivel_joint" type="continuous">
+		<origin xyz="-0.185 0 0.055" rpy="0 0 0"/>
+		<anchor xyz="0 0 0"/>
+    	<limit effort="100" velocity="100" k_velocity="0" />
+    	<joint_properties damping="0.0" friction="0.0" />
+		<parent link="base_link"/>
+		<child link="caster_swivel"/>
+	</joint>
+	<link name="caster_swivel">
+		<inertial>
+			<mass value="0.1"/>	
+			<origin xyz="0 0 0"/>
+			<inertia ixx="0.01" ixy="0" ixz="0"
+				 iyy="0.01" iyz="0" izz="0.01"/>
+  	 	</inertial>
+	<visual>
+		<origin xyz="0 0 0" rpy="0 0 0"/>
+		<geometry name="pioneer_geom">
+			<mesh filename="$(find pioneer_description)/meshes/caster_swivel.stl"/>
+		</geometry>
+		<material name="caster_swivel">
+			<color rgba="0.5 0.5 0.5 1"/>
+		</material>
+	</visual>
+	</link>
+	<gazebo reference="caster_swivel">
+		<material value="Gazebo/Grey"/>
+	</gazebo>
+
+<!-- Center Wheel + Hubcap -->
+	<link name="caster_hubcap">
+		<inertial>
+			<mass value="0.01"/>	
+			<origin xyz="0 0 0"/>
+			<inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983"
+				 iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
+  	 	</inertial>
+	<visual>
+		<origin xyz="0 0 0" rpy="0 0 0"/>
+		<geometry name="pioneer_geom">
+			<mesh filename="$(find pioneer_description)/meshes/caster_hubcap.stl"/>
+		</geometry>
+		<material name="caster_swivel">
+			<color rgba="0.5 0.5 0.5 1"/>
+		</material>
+	</visual>
+	<collision>
+		<origin xyz="0 0 0" rpy="0 0 0"/>
+		<geometry>
+			<box size="0.05 0.05 0.05"/>
+		</geometry>
+	</collision>
+	</link>
+	<gazebo reference="caster_hubcap">
+		<material value="Gazebo/Grey"/>
+	</gazebo>
+
+	<joint name="caster_swivel_hubcap_joint" type="continuous">
+		<origin xyz="-0.026 0 -0.016" rpy="0 0 0"/>
+		<axis xyz="0 1 0"/>
+		<anchor xyz="0 0 0"/>
+    <limit effort="100" velocity="100" k_velocity="0" />
+    <joint_properties damping="0.0" friction="0.0" />
+	  <parent link="caster_swivel"/>
+		<child link="caster_wheel"/>
+	</joint>
+
+	<link name="caster_wheel">
+		<inertial>
+			<mass value="0.1"/>	
+			<origin xyz="0 0 0"/>
+			<inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983"
+				 iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
+  	 	</inertial>
+	<visual>
+		<origin xyz="0 0 0" rpy="0 0 0"/>
+		<geometry name="pioneer_geom">
+			<mesh filename="$(find pioneer_description)/meshes/caster_wheel.stl"/>
+		</geometry>
+		<material name="WheelBlack">
+			<color rgba="0.117 0.117 0.117 1"/>
+		</material>
+	</visual>
+	<collision>
+		<origin xyz="0 0 0" rpy="${-3.1415927/2.0} 0 0"/>
+		<geometry>
+			<!--<mesh filename="$(find pioneer_description)/meshes/caster_wheel.stl"/>-->
+			<cylinder radius="0.0375" length="0.01"/>
+		</geometry>
+	</collision>
+	</link>
+	<gazebo reference="left_hub">
+		<material value="Gazebo/Yellow"/>
+	</gazebo>
+
+	<joint name="caster_wheel_joint" type="fixed">
+		<origin xyz="-0.0035 0 -0.001" rpy="0 0 0"/>
+		<parent link="caster_wheel"/>
+		<child link="caster_hubcap"/>
+	</joint>
+
+	<link name="left_hub">
+		<visual>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<geometry>
+				<mesh filename="$(find pioneer_description)/meshes/left_hubcap.stl"/>
+			</geometry>
+			<material name="HubcapYellow">
+				<color rgba="1.0 0.811 0.151 1.0"/>
+			</material>
+		</visual>
+		<collision>
+			<origin xyz="0 0 0" rpy="${-3.1415927/2.0} 0 0"/>
+			<geometry>
+				<!--<mesh filename="$(find pioneer_description)/meshes/caster_wheel.stl"/>-->
+				<cylinder radius="0.09" length="0.01"/>
+			</geometry>
+		</collision>
+	</link>
+
+	<joint name="left_hub_joint" type="fixed">
+		<origin xyz="0 0.15 0.08" rpy="0 0 0"/>
+		<parent link="base_link"/>
+		<child link="left_hub"/>
+		<axis xyz="0 1 0"/>
+	</joint>
+
+	<link name="left_wheel">
+		<visual>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<geometry>
+				<mesh filename="$(find pioneer_description)/meshes/left_wheel.stl"/>
+			</geometry>
+			<material name="WheelBlack">
+				<color rgba="0.117 0.117 0.117 1"/>
+			</material>
+		</visual>
+	</link>
+
+	<joint name="left_wheel_joint" type="continuous"> <!-- type="continuous" -->
+		<origin xyz="0 0 0" rpy="0 0 0"/>
+		<parent link="left_hub"/>
+		<child link="left_wheel"/>
+	</joint>
+	
+	<gazebo reference="left_wheel_joint">
+		<material value="Gazebo/Black"/>
+	</gazebo>
+
+	<link name="right_hub">
+		<visual>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<geometry>
+				<mesh filename="$(find pioneer_description)/meshes/right_hubcap.stl"/>
+			</geometry>
+			<material name="HubcapYellow">
+				<color rgba="1.0 0.811 0.151 1.0"/>
+			</material>
+		</visual>
+		<collision>
+		<origin xyz="0 0 0" rpy="${-3.1415927/2.0} 0 0"/>
+		<geometry>
+			<!--<mesh filename="$(find pioneer_description)/meshes/caster_wheel.stl"/>-->
+			<cylinder radius="0.09" length="0.01"/>
+		</geometry>
+	</collision>
+	</link>
+	<gazebo reference="right_hub">
+		<material value="Gazebo/Yellow"/>
+	</gazebo>
+
+	<joint name="right_hub_joint" type="fixed">
+		<origin xyz="0 -0.15 0.08" rpy="0 0 0"/>
+		<parent link="base_link"/>
+		<child link="right_hub"/>
+		<axis xyz="0 1 0"/>
+	</joint>
+
+	<link name="right_wheel">
+		<visual>
+			<origin xyz="0 0 0" rpy="0 0 0"/>
+			<geometry>
+				<mesh filename="$(find pioneer_description)/meshes/right_wheel.stl"/>
+			</geometry>
+			<material name="WheelBlack">
+				<color rgba="0.117 0.117 0.117 1"/>
+			</material>
+		</visual>
+	</link>
+
+	<joint name="right_wheel_joint" type="continuous"> <!-- type="continuous" -->
+		<origin xyz="0 0 0" rpy="0 0 0"/>
+		<parent link="right_hub"/>
+		<child link="right_wheel"/>
+	</joint>
+	<gazebo reference="right_wheel_joint">
+		<material value="Gazebo/Black"/>
+	</gazebo>
+
+
+	
+
+	<create>
+	<back_sonar parent="base_link"/>
+  	<top_plate  parent="base_link"/>
+  	</create>
+
+<gazebo>
+	<plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
+    <alwaysOn>true</alwaysOn>
+		<updateRate>100</updateRate>
+		<leftJoint>left_wheel_joint</leftJoint>
+		<rightJoint>right_wheel_joint</rightJoint>
+		<wheelSeparation>0.158</wheelSeparation>
+		<wheelDiameter>0.12</wheelDiameter>
+		<torque>5</torque>
+		<!-- interface:position name="position_iface_0"/ -->
+    <commandTopic>cmd_vel</commandTopic>
+    <robotBaseFrame>base_link</robotBaseFrame>
+    <odometryTopic>odom</odometryTopic>
+    <odometryFrame>odom</odometryFrame>
+	</plugin>
+
+  <!-- TODO include P3D (ground truth) plugin -->
+  
+
+  <!-- XXX old urdf included a gazebo_ros_controller_manager plugin with a 1 second update rate -->
+</gazebo>
+
+</robot>
\ No newline at end of file
diff --git a/src/pioneer_description/urdf/pioneer3dx_wheel.xacro b/src/pioneer_description/urdf/pioneer3dx_wheel.xacro
new file mode 100644
index 0000000..5944bc4
--- /dev/null
+++ b/src/pioneer_description/urdf/pioneer3dx_wheel.xacro
@@ -0,0 +1,108 @@
+<?xml version="1.0"?>
+
+<robot
+  xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+  xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+  xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+  xmlns:xacro="http://ros.org/wiki/xacro"
+  >
+
+  <!--<include filename="$(find amr_robots_description)/urdf/materials.xacro"/>-->
+
+  <!-- Properties (Constants) -->
+  <property name="M_PI" value="3.14159"/>
+
+<!-- Right/Left Hubcap + Wheel -->
+<xacro:macro name="p3dx_wheel" params="suffix reflect">
+  <link name="p3dx_${suffix}_wheel">
+    <inertial>
+      <mass value="0.5"/> 
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.012411765597" ixy="0" ixz="0"
+         iyy="0.015218160428" iyz="0" izz="0.011763977943"/>
+      </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry name="pioneer_geom">
+        <mesh filename="$(find pioneer_description)/meshes/meshes/p3dx_meshes/${suffix}_wheel.stl"/>
+      </geometry>
+      <material name="WheelBlack">
+        <color rgba="0.117 0.117 0.117 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="${-3.1415927/2} 0 0"/>
+      <geometry>
+        <!--<mesh filename="$(find pioneer_description)/meshes/meshes/p3dx_meshes/${suffix}_wheel.stl"/>-->
+        <cylinder radius="0.09" length="0.01"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="base_${suffix}_hubcap_joint" type="fixed">
+    <!--<origin xyz="0 ${reflect*0.158} 0.091" rpy="0 0 0"/>-->
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+
+    <parent link="p3dx_${suffix}_wheel"/>
+    <child link="p3dx_${suffix}_hubcap"/>
+  </joint>
+
+  <link name="p3dx_${suffix}_hubcap">
+    <inertial>
+      <mass value="0.01"/>  
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.012411765597" ixy="0" ixz="0"
+         iyy="0.015218160428" iyz="0" izz="0.011763977943"/>
+      </inertial>
+  <visual>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <geometry name="pioneer_geom">
+      <mesh filename="$(find pioneer_description)/meshes/meshes/p3dx_meshes/${suffix}_hubcap.stl"/>
+    </geometry>
+    <material name="HubcapYellow">
+      <color rgba="1.0 0.811 0.151 1.0"/>
+    </material>
+  </visual>
+  <collision>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <geometry>
+      <box size="0 0 0"/>
+    </geometry>
+  </collision>
+  </link>
+
+  <transmission type="pr2_mechanism_model/SimpleTransmission" name="${suffix}_wheel_trans">
+    <actuator name="base_${suffix}_wheel_motor" />
+    <joint name="base_${suffix}_wheel_joint" />
+    <mechanicalReduction>${reflect * 624/35 * 80/19}</mechanicalReduction>
+  </transmission>
+
+  <joint name="base_${suffix}_wheel_joint" type="continuous">
+    <axis xyz="0 0 1"/>
+    <anchor xyz="0 0 0"/>
+    <limit effort="100" velocity="100" />
+    <joint_properties damping="0.0" friction="0.0" />
+    <origin xyz="0 ${reflect*0.158} 0.091" rpy="0 0 0"/>
+    <parent link="p3dx_${suffix}_hubcap"/>
+    <child link="p3dx_${suffix}_wheel"/>
+  </joint>
+
+
+  <gazebo reference="p3dx_${suffix}_hubcap">
+      <material value="Gazebo/Yellow"/>
+  </gazebo>
+
+
+  <gazebo reference="p3dx_${suffix}_wheel">
+      <material value="Gazebo/Black"/>
+      <elem key="mu1" value="0.5" />
+      <elem key="mu2" value="50.0" />
+      <elem key="kp"  value="100000000.0" />
+      <elem key="kd"  value="1.0" />
+  </gazebo>
+
+</xacro:macro>
+
+
+</robot>
+
diff --git a/src/ros2aria/include/ros2aria/ros2aria.hpp b/src/ros2aria/include/ros2aria/ros2aria.hpp
index 7b3285a..d6471ca 100644
--- a/src/ros2aria/include/ros2aria/ros2aria.hpp
+++ b/src/ros2aria/include/ros2aria/ros2aria.hpp
@@ -1,4 +1,6 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <utility>
+#include<string>
 
 #include "ros2aria/raiibot.hpp"
 #include <Aria/Aria.h>
@@ -13,6 +15,8 @@
 #include "std_msgs/msg/float32.hpp"
 #include "std_msgs/msg/int8.hpp"
 #include "std_srvs/srv/empty.hpp"
+#include "tf2_ros/transform_broadcaster.h"
+#include "tf2/LinearMath/Quaternion.h"
 
 #define UNUSED(x) (void)(x)
 
@@ -27,9 +31,6 @@ private:
     RAIIBot::SharedPtr robot;
     ArFunctorC<Ros2Aria> sensorCb;
 
-    // config
-    std::size_t pioneer_id;
-
     // publishers
     void publish();
 
@@ -39,9 +40,11 @@ private:
     void publishSonar(sensor_msgs::msg::PointCloud cloud);
     void publishSonarPointCloud2(sensor_msgs::msg::PointCloud cloud);
 
-    nav_msgs::msg::Odometry handlePose(rclcpp::Time stamp);
+    std::pair<nav_msgs::msg::Odometry, geometry_msgs::msg::TransformStamped> handlePose(rclcpp::Time stamp);
+    std::unique_ptr<tf2_ros::TransformBroadcaster> odom_tf_broadcaster_;
     rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pose_pub_;
     void publishPose(nav_msgs::msg::Odometry pose);
+    void publishTf(geometry_msgs::msg::TransformStamped tf);
 
     sensor_msgs::msg::JointState handleWheels(rclcpp::Time stamp);
     rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr wheels_pub_;
@@ -50,7 +53,7 @@ private:
     rclcpp::Publisher<std_msgs::msg::Int8>::SharedPtr battery_recharge_state_pub_;
     rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr battery_state_of_charge_pub_;
     rclcpp::Publisher<ros2aria_msgs::msg::RobotInfoMsg>::SharedPtr robot_info_pub_;
-    void publishState(rclcpp::Time stamp);
+    void publishState();
 
     // subscribers
     rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
diff --git a/src/ros2aria/src/pose.cpp b/src/ros2aria/src/pose.cpp
index 355cda3..6387dce 100644
--- a/src/ros2aria/src/pose.cpp
+++ b/src/ros2aria/src/pose.cpp
@@ -1,6 +1,6 @@
 #include "ros2aria/ros2aria.hpp"
 
-nav_msgs::msg::Odometry Ros2Aria::handlePose(rclcpp::Time stamp) {
+std::pair<nav_msgs::msg::Odometry, geometry_msgs::msg::TransformStamped> Ros2Aria::handlePose(rclcpp::Time stamp) {
     nav_msgs::msg::Odometry odom_msg;
 
     auto r = robot->getRobot();
@@ -19,11 +19,20 @@ nav_msgs::msg::Odometry Ros2Aria::handlePose(rclcpp::Time stamp) {
     odom_msg.twist.twist.linear.y = r->getLatVel() / 1000;
     odom_msg.twist.twist.angular.z = r->getRotVel() * M_PI / 180;
 
-    odom_msg.header.frame_id = "odom";
-    odom_msg.child_frame_id = "base_link";
+    odom_msg.header.frame_id = std::string(get_namespace()) + "/odom";
+    odom_msg.child_frame_id = std::string(get_namespace()) + "/base_link";
     odom_msg.header.stamp = stamp;
 
-    return odom_msg;
+    geometry_msgs::msg::TransformStamped transform;
+    transform.header = odom_msg.header;
+    transform.child_frame_id= odom_msg.child_frame_id;
+    transform.transform.translation.x = odom_msg.pose.pose.position.x;
+    transform.transform.translation.y = odom_msg.pose.pose.position.y;
+    transform.transform.translation.z = odom_msg.pose.pose.position.z;
+    transform.transform.rotation = odom_msg.pose.pose.orientation;
+
+    std::pair<nav_msgs::msg::Odometry, geometry_msgs::msg::TransformStamped> pair_msgs = {odom_msg, transform};
+    return pair_msgs;
 }
 
 void Ros2Aria::publishPose(nav_msgs::msg::Odometry pose) {
@@ -31,4 +40,8 @@ void Ros2Aria::publishPose(nav_msgs::msg::Odometry pose) {
         return;
 
     this->pose_pub_->publish(pose);
+}
+
+void Ros2Aria::publishTf(geometry_msgs::msg::TransformStamped tf){
+    odom_tf_broadcaster_->sendTransform(tf);
 }
\ No newline at end of file
diff --git a/src/ros2aria/src/publish.cpp b/src/ros2aria/src/publish.cpp
index ef2715a..ec02995 100644
--- a/src/ros2aria/src/publish.cpp
+++ b/src/ros2aria/src/publish.cpp
@@ -9,11 +9,12 @@ void Ros2Aria::publish()
     publishSonar(sonarData);
     publishSonarPointCloud2(sonarData);
 
-    nav_msgs::msg::Odometry pose = handlePose(t);
-    publishPose(pose);
+    auto pose = handlePose(t);
+    publishPose(pose.first);
+    publishTf(pose.second);
 
     sensor_msgs::msg::JointState wheels = handleWheels(t);
     publishWheels(wheels);
 
-    publishState(t);
+    publishState();
 }
\ No newline at end of file
diff --git a/src/ros2aria/src/ros2aria.cpp b/src/ros2aria/src/ros2aria.cpp
index c6d778d..5292c91 100644
--- a/src/ros2aria/src/ros2aria.cpp
+++ b/src/ros2aria/src/ros2aria.cpp
@@ -19,8 +19,9 @@ Ros2Aria::Ros2Aria()
     sonar_pointcloud2_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("sonar_pointcloud2", 10);
 
     pose_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("odom/wheels", 1000);
+    odom_tf_broadcaster_ =std::make_unique<tf2_ros::TransformBroadcaster>(*this);
 
-    wheels_pub_ = this->create_publisher<sensor_msgs::msg::JointState>("wheels", 1000);
+    wheels_pub_ = this->create_publisher<sensor_msgs::msg::JointState>("joint_states", 1000);
 
     battery_recharge_state_pub_ = this->create_publisher<std_msgs::msg::Int8>("battery_recharge_state", 10);
     battery_state_of_charge_pub_ = this->create_publisher<std_msgs::msg::Float32>("battery_state_of_charge", 10);
@@ -37,9 +38,7 @@ Ros2Aria::Ros2Aria()
     auto r = robot->getRobot();
     r->addSensorInterpTask("ROSPublishingTask", 100, &this->sensorCb);
 
-    declare_parameter<uint8_t>("pioneer_id");
-    pioneer_id = get_parameter("pioneer_id").as_int();
-    RCLCPP_INFO(get_logger(), "PIONEER_ID = %d", pioneer_id);
+    RCLCPP_INFO(get_logger(), "NAMESPACE = %s", get_namespace());
 }
 
 Ros2Aria::~Ros2Aria()
diff --git a/src/ros2aria/src/state.cpp b/src/ros2aria/src/state.cpp
index eca3365..93c71a5 100644
--- a/src/ros2aria/src/state.cpp
+++ b/src/ros2aria/src/state.cpp
@@ -1,6 +1,6 @@
 #include "ros2aria/ros2aria.hpp"
 
-void Ros2Aria::publishState(rclcpp::Time stamp)
+void Ros2Aria::publishState()
 {
     auto r = this->robot->getRobot();
 
@@ -18,8 +18,7 @@ void Ros2Aria::publishState(rclcpp::Time stamp)
     {
         ros2aria_msgs::msg::RobotInfoMsg robot_info_msg;
 
-        // TODO: allow setting the robot_id
-        robot_info_msg.robot_id.data = pioneer_id;
+        robot_info_msg.robot_id.data =  static_cast<uint8_t>(std::string(get_namespace()).back());
         robot_info_msg.battery_voltage.data = r->getRealBatteryVoltageNow();
         robot_info_msg.twist.linear.x = r->getVel() / 1000;
         robot_info_msg.twist.linear.y = r->getLatVel() / 1000.0;
diff --git a/src/ros2aria/src/wheels.cpp b/src/ros2aria/src/wheels.cpp
index a1a827f..fee4114 100644
--- a/src/ros2aria/src/wheels.cpp
+++ b/src/ros2aria/src/wheels.cpp
@@ -13,8 +13,9 @@ sensor_msgs::msg::JointState Ros2Aria::handleWheels(rclcpp::Time stamp)
     wheels.velocity.resize(2);
     wheels.effort.resize(0);
 
-    wheels.name[0] = "Wheel_L";
-    wheels.name[1] = "Wheel_R";
+    // robot_state_publisher gives namespace
+    wheels.name[0] = "left_wheel_joint";
+    wheels.name[1] =  "right_wheel_joint";
     wheels.position[0] = r->getLeftEncoder();
     wheels.position[1] = r->getRightEncoder();
     wheels.velocity[0] = r->getLeftVel();