From 7c3810761d12f064f15459458529ec20ff28f89d Mon Sep 17 00:00:00 2001
From: Michel Hidalgo <michel@ekumenlabs.com>
Date: Fri, 8 Nov 2019 10:24:05 -0300
Subject: [PATCH 1/2] [DO NOT MERGE] Adds basic swarm controller node (#113)

* Add ca_swarm package.

Include basic swarm formation controller node.

* Get swarm_controller_node hooked up in launch.

* Add missing numpy module alias.

* Clean up ca_swarm CMakeLists.txt

* Use base geometry_msgs.msg.Twist message, w/o stamps.

* Clean up ca_swarm swarm_controller_node.

* Clean up cooperative two robots sim demo.

* Fix Michel's approach
---
 .../create_cooperative_two_rooms.launch       |  15 +-
 ca_gazebo/launch/spawn_multirobot.launch      |   2 +-
 ca_gazebo/worlds/two_rooms.world              |  24 +-
 ca_tools/rviz/swarm.rviz                      | 492 ++++++++++++++++++
 navigation/ca_swarm/CMakeLists.txt            |  21 +
 navigation/ca_swarm/package.xml               |   4 +
 navigation/ca_swarm/scripts/box_client.py     |   8 +-
 .../ca_swarm/scripts/swarm_controller_node    | 135 +++++
 8 files changed, 676 insertions(+), 25 deletions(-)
 create mode 100644 ca_tools/rviz/swarm.rviz
 create mode 100755 navigation/ca_swarm/scripts/swarm_controller_node

diff --git a/ca_gazebo/launch/create_cooperative_two_rooms.launch b/ca_gazebo/launch/create_cooperative_two_rooms.launch
index 75d352890..6e6bc0b1a 100644
--- a/ca_gazebo/launch/create_cooperative_two_rooms.launch
+++ b/ca_gazebo/launch/create_cooperative_two_rooms.launch
@@ -1,11 +1,16 @@
 <launch>
-  <param name="create1/amcl/initial_pose_x" value="$(optenv X1 -1)"/>
-  <param name="create1/amcl/initial_pose_y" value="$(optenv Y1 1)"/>
+  <param name="create1/amcl/initial_pose_x" value="$(optenv X1 5.74)"/>
+  <param name="create1/amcl/initial_pose_y" value="$(optenv Y1 0.96)"/>
   <param name="create1/amcl/initial_pose_a" value="$(optenv A1 0)"/>
 
-  <param name="create2/amcl/initial_pose_x" value="$(optenv X2 1)"/>
-  <param name="create2/amcl/initial_pose_y" value="$(optenv Y2 1)"/>
-  <param name="create2/amcl/initial_pose_a" value="$(optenv A2 3.1416)"/>
+  <param name="create2/amcl/initial_pose_x" value="$(optenv X2 5.74)"/>
+  <param name="create2/amcl/initial_pose_y" value="$(optenv Y2 0.27)"/>
+  <param name="create2/amcl/initial_pose_a" value="$(optenv A2 0)"/>
+
+  <node pkg="ca_swarm" type="swarm_controller_node" name="swarm_controller_node" output="screen">
+    <rosparam param="swarm_robots">[create1, create2]</rosparam>
+    <param name="body_frame" value="unit_box"/>
+  </node>
 
   <include file="$(find ca_gazebo)/launch/create_empty_world.launch">
     <arg name="env" value="two_rooms"/>
diff --git a/ca_gazebo/launch/spawn_multirobot.launch b/ca_gazebo/launch/spawn_multirobot.launch
index 243197f0a..e9f649886 100644
--- a/ca_gazebo/launch/spawn_multirobot.launch
+++ b/ca_gazebo/launch/spawn_multirobot.launch
@@ -22,7 +22,7 @@
 
     <!-- RViz -->
     <include if="$(arg rviz)" file="$(find ca_tools)/launch/rviz.launch">
-      <arg name="config_file" value="$(find ca_tools)/rviz/multi_robot.rviz"/>
+      <arg name="config_file" value="$(find ca_tools)/rviz/swarm.rviz"/>
     </include>
   </group>
 
diff --git a/ca_gazebo/worlds/two_rooms.world b/ca_gazebo/worlds/two_rooms.world
index d2eeb7f72..4130b6757 100644
--- a/ca_gazebo/worlds/two_rooms.world
+++ b/ca_gazebo/worlds/two_rooms.world
@@ -996,7 +996,7 @@
       <pose frame=''>6.42455 0.596341 0.5 0 -0 0</pose>
       <link name='link'>
         <inertial>
-          <mass>1</mass>
+          <mass>5</mass>
           <inertia>
             <ixx>0.166667</ixx>
             <ixy>0</ixy>
@@ -1010,22 +1010,22 @@
         <collision name='collision'>
           <geometry>
             <box>
-              <size>1 1 1</size>
+              <size>1 1 0.5</size>
             </box>
           </geometry>
           <max_contacts>10</max_contacts>
           <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
+              <friction>
+                <ode>
+                  <mu>0.7</mu>
+                  <mu2>0.7</mu2>
+                </ode>
+              </friction>
+              <bounce/>
+              <contact>
                 <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
+              </contact>
+            </surface>
         </collision>
         <visual name='visual'>
           <geometry>
diff --git a/ca_tools/rviz/swarm.rviz b/ca_tools/rviz/swarm.rviz
new file mode 100644
index 000000000..79179d552
--- /dev/null
+++ b/ca_tools/rviz/swarm.rviz
@@ -0,0 +1,492 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 141
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /TF1/Frames1
+      Splitter Ratio: 0.680555582
+    Tree Height: 705
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679016
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.0299999993
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        create1_tf/base_footprint:
+          Value: false
+        create1_tf/base_link:
+          Value: true
+        create1_tf/block_link:
+          Value: false
+        create1_tf/caster_wheel_link:
+          Value: false
+        create1_tf/front_left_cliff_sensor_link:
+          Value: false
+        create1_tf/front_right_cliff_sensor_link:
+          Value: false
+        create1_tf/imu_link:
+          Value: false
+        create1_tf/laser_link:
+          Value: false
+        create1_tf/left_wheel_link:
+          Value: false
+        create1_tf/odom:
+          Value: false
+        create1_tf/raspicam_link:
+          Value: false
+        create1_tf/raspicam_optical_frame:
+          Value: false
+        create1_tf/right_wheel_link:
+          Value: false
+        create1_tf/side_left_cliff_sensor_link:
+          Value: false
+        create1_tf/side_right_cliff_sensor_link:
+          Value: false
+        create1_tf/wall_sensor_link:
+          Value: false
+        create2_tf/base_footprint:
+          Value: false
+        create2_tf/base_link:
+          Value: true
+        create2_tf/block_link:
+          Value: false
+        create2_tf/caster_wheel_link:
+          Value: false
+        create2_tf/front_left_cliff_sensor_link:
+          Value: false
+        create2_tf/front_right_cliff_sensor_link:
+          Value: false
+        create2_tf/imu_link:
+          Value: false
+        create2_tf/laser_link:
+          Value: false
+        create2_tf/left_wheel_link:
+          Value: false
+        create2_tf/odom:
+          Value: false
+        create2_tf/raspicam_link:
+          Value: false
+        create2_tf/raspicam_optical_frame:
+          Value: false
+        create2_tf/right_wheel_link:
+          Value: false
+        create2_tf/side_left_cliff_sensor_link:
+          Value: false
+        create2_tf/side_right_cliff_sensor_link:
+          Value: false
+        create2_tf/wall_sensor_link:
+          Value: false
+        door:
+          Value: false
+        map:
+          Value: false
+        unit_box:
+          Value: true
+        unit_cylinder:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        map:
+          create1_tf/odom:
+            create1_tf/base_footprint:
+              create1_tf/base_link:
+                create1_tf/block_link:
+                  create1_tf/laser_link:
+                    {}
+                  create1_tf/raspicam_link:
+                    create1_tf/raspicam_optical_frame:
+                      {}
+                create1_tf/caster_wheel_link:
+                  {}
+                create1_tf/front_left_cliff_sensor_link:
+                  {}
+                create1_tf/front_right_cliff_sensor_link:
+                  {}
+                create1_tf/imu_link:
+                  {}
+                create1_tf/left_wheel_link:
+                  {}
+                create1_tf/right_wheel_link:
+                  {}
+                create1_tf/side_left_cliff_sensor_link:
+                  {}
+                create1_tf/side_right_cliff_sensor_link:
+                  {}
+                create1_tf/wall_sensor_link:
+                  {}
+          create2_tf/odom:
+            create2_tf/base_footprint:
+              create2_tf/base_link:
+                create2_tf/block_link:
+                  create2_tf/laser_link:
+                    {}
+                  create2_tf/raspicam_link:
+                    create2_tf/raspicam_optical_frame:
+                      {}
+                create2_tf/caster_wheel_link:
+                  {}
+                create2_tf/front_left_cliff_sensor_link:
+                  {}
+                create2_tf/front_right_cliff_sensor_link:
+                  {}
+                create2_tf/imu_link:
+                  {}
+                create2_tf/left_wheel_link:
+                  {}
+                create2_tf/right_wheel_link:
+                  {}
+                create2_tf/side_left_cliff_sensor_link:
+                  {}
+                create2_tf/side_right_cliff_sensor_link:
+                  {}
+                create2_tf/wall_sensor_link:
+                  {}
+          door:
+            {}
+          unit_box:
+            {}
+          unit_cylinder:
+            {}
+      Update Interval: 0
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_footprint:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        block_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        caster_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_left_cliff_sensor_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        front_right_cliff_sensor_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        laser_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        left_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        raspicam_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        raspicam_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        right_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        side_left_cliff_sensor_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        side_right_cliff_sensor_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        wall_sensor_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: create1_tf
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Alpha: 0.699999988
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: false
+      Name: Map
+      Topic: /map
+      Unreliable: false
+      Use Timestamp: false
+      Value: false
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_footprint:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        block_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        caster_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_left_cliff_sensor_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        front_right_cliff_sensor_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        laser_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        left_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        raspicam_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        raspicam_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        right_wheel_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        side_left_cliff_sensor_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        side_right_cliff_sensor_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        wall_sensor_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: create2_tf
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Alpha: 0.699999988
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: false
+      Enabled: false
+      Name: Local Costmap 2
+      Topic: /create2/move_base/local_costmap/costmap
+      Unreliable: false
+      Use Timestamp: false
+      Value: false
+    - Alpha: 0.699999988
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: false
+      Enabled: false
+      Name: Local Costmap 1
+      Topic: /create1/move_base/local_costmap/costmap
+      Unreliable: false
+      Use Timestamp: false
+      Value: false
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 255; 0; 0
+      Enabled: true
+      Head Diameter: 0.300000012
+      Head Length: 0.200000003
+      Length: 0.300000012
+      Line Style: Lines
+      Line Width: 0.0299999993
+      Name: Global Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.0299999993
+      Shaft Diameter: 0.100000001
+      Shaft Length: 0.100000001
+      Topic: /collaborative_move_base/NavfnROS/plan
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.100000001
+      Class: rviz/Pose
+      Color: 85; 0; 255
+      Enabled: true
+      Head Length: 0.300000012
+      Head Radius: 0.100000001
+      Name: Pose
+      Shaft Length: 1
+      Shaft Radius: 0.0500000007
+      Shape: Arrow
+      Topic: /collaborative_move_base/current_goal
+      Unreliable: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /create1/initialpose
+    - Class: rviz/SetGoal
+      Topic: /create1/move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 0.680082142
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 6.47536421
+        Y: 1.39989793
+        Z: 7.19870234
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.0500000007
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.00999999978
+      Pitch: 1.56979632
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 1.48539829
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1056
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000388fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003a00000388000000c600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006100000002dd000000e50000000000000000000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c40000009e00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000006100000038800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1920
+  X: 0
+  Y: 24
diff --git a/navigation/ca_swarm/CMakeLists.txt b/navigation/ca_swarm/CMakeLists.txt
index 5cff95479..63354c9f4 100644
--- a/navigation/ca_swarm/CMakeLists.txt
+++ b/navigation/ca_swarm/CMakeLists.txt
@@ -15,8 +15,29 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   tf
 )
+
+###################################
+## catkin specific configuration ##
+###################################
 catkin_package()
 
+###########
+## Build ##
+###########
+
 include_directories(
   ${catkin_INCLUDE_DIRS}
+  geometry_msgs
+  rospy
+)
+
+#############
+## Install ##
+#############
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+install(PROGRAMS
+  scripts/swarm_controller_node
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
diff --git a/navigation/ca_swarm/package.xml b/navigation/ca_swarm/package.xml
index fe39132e3..a407b77fa 100644
--- a/navigation/ca_swarm/package.xml
+++ b/navigation/ca_swarm/package.xml
@@ -20,14 +20,18 @@
   <build_export_depend>move_base_msgs</build_export_depend>
   <build_export_depend>roscpp</build_export_depend>
   <build_export_depend>tf</build_export_depend>
+  <build_export_depend>tf2_ros</build_export_depend>
   <exec_depend>actionlib</exec_depend>
   <exec_depend>amcl</exec_depend>
   <exec_depend>ca_move_base</exec_depend>
   <exec_depend>dwa_local_planner</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>move_base</exec_depend>
   <exec_depend>move_base_msgs</exec_depend>
   <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
   <exec_depend>tf</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
   <exec_depend>map_server</exec_depend>
 
   <export>
diff --git a/navigation/ca_swarm/scripts/box_client.py b/navigation/ca_swarm/scripts/box_client.py
index a7296851e..1a45629da 100755
--- a/navigation/ca_swarm/scripts/box_client.py
+++ b/navigation/ca_swarm/scripts/box_client.py
@@ -20,7 +20,7 @@ def __init__(self):
 
         # Append each of the four waypoints to the list.  Each waypoint
         # is a pose consisting of a position and orientation in the map frame.
-        waypoints.append(Pose(Point(1, 1, 0.0), Quaternion(0,0,0,1)));
+        waypoints.append(Pose(Point(10.0, 0.62, 0.0), Quaternion(0,0,0,1)))
 
         # Initialize the visualization markers for RViz
         self.init_markers()
@@ -31,9 +31,6 @@ def __init__(self):
             p = waypoint.position
             self.markers.points.append(p)
 
-        # Publisher to manually control the robot (e.g. to stop it)
-        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
-
         # Subscribe to the move_base action server
         self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
 
@@ -121,9 +118,6 @@ def shutdown(self):
         # Cancel any active goals
         self.move_base.cancel_goal()
         rospy.sleep(2)
-        # Stop the robot
-        self.cmd_vel_pub.publish(Twist())
-        rospy.sleep(1)
 
 if __name__ == '__main__':
     try:
diff --git a/navigation/ca_swarm/scripts/swarm_controller_node b/navigation/ca_swarm/scripts/swarm_controller_node
new file mode 100755
index 000000000..a547bd807
--- /dev/null
+++ b/navigation/ca_swarm/scripts/swarm_controller_node
@@ -0,0 +1,135 @@
+#! /usr/bin/env python
+
+from collections import namedtuple
+import numpy as np
+
+import rospy
+import tf2_ros
+
+from tf import transformations
+
+from geometry_msgs.msg import Twist
+
+class SwarmRobotProxy(object):
+
+    def __init__(self, name, frame, cmd_vel, initial_tf):
+        self.name = name
+        self.frame = frame
+        self.cmd_vel = cmd_vel
+        self.initial_tf = initial_tf
+
+class SwarmControllerNode(object):
+
+    def __init__(self):
+        rospy.init_node('swarm_controller')
+        robot_frame_suffix = rospy.get_param(
+            '~robot_frame_suffix', '_tf/base_link'
+        )
+        self._swarm_robots = [
+            SwarmRobotProxy(
+                name=robot_name,
+                frame=robot_name + robot_frame_suffix,
+                cmd_vel=rospy.Publisher(
+                    robot_name + '/cmd_vel', Twist, queue_size=10
+                ),
+                initial_tf=transformations.identity_matrix()
+            ) for robot_name in rospy.get_param('~swarm_robots')
+        ]
+
+        self._tf_buffer = tf2_ros.Buffer()
+        self._tf_listener = tf2_ros.TransformListener(self._tf_buffer)
+        self._body_frame = rospy.get_param('~body_frame')
+
+        self._swarm_cmd_vel_sub = rospy.Subscriber(
+            'cmd_vel', Twist, self._on_swarm_cmd_vel
+        )
+
+    def _on_swarm_cmd_vel(self, swarm_twist):
+        v = swarm_twist.linear.x
+        w = swarm_twist.angular.z
+        R = v / w if w != 0. else np.inf
+        for robot in self._swarm_robots:
+            robot_twist = Twist()
+            try:
+                robot_transform_in_body_frame = self._tf_buffer.lookup_transform(
+                    self._body_frame, robot.frame, rospy.Time()
+                ).transform
+
+                x = robot_transform_in_body_frame.translation.x
+                y = robot_transform_in_body_frame.translation.y
+                _, _, yaw = transformations.euler_from_quaternion([
+                    robot_transform_in_body_frame.rotation.x,
+                    robot_transform_in_body_frame.rotation.y,
+                    robot_transform_in_body_frame.rotation.z,
+                    robot_transform_in_body_frame.rotation.w
+                ])
+
+                if np.allclose(robot.initial_tf, np.identity(4, dtype=np.float64)):
+                  robot.initial_tf = transformations.compose_matrix(angles=[0., 0., yaw], translate=[x, y, 0.])
+                else:
+                  new_tf = transformations.compose_matrix(angles=[0, 0, yaw], translate=[x, y, 0])
+                  tf_error = transformations.concatenate_matrices(
+                    transformations.inverse_matrix(robot.initial_tf),
+                    new_tf
+                  )
+                  [tx, ty, _] = transformations.translation_from_matrix(tf_error)
+                  [_, _, rz] = transformations.euler_from_matrix(tf_error)
+
+
+                  linear_k_ = 0.02 # Scale linear velocity, chosen by trial and error
+                  angular_k_ = 0.08 # Scale angular velocity, chosen by trial and error
+                  # Bad readings, stop
+                  if np.isclose(rz, 0., rtol=1e-3, atol=1e-3) and \
+                     np.isclose(tx, 0., rtol=1e-3, atol=1e-3) and \
+                     np.isclose(ty, 0., rtol=1e-3, atol=1e-3):
+                      x_box = 0
+                      w_box = 0
+                  elif np.isclose(tx, 0., rtol=1e-3, atol=1e-3) and \
+                       np.isclose(ty, 0., rtol=1e-3, atol=1e-3):
+                      # Too close, just rotate
+                      x_box = 0
+                      w_box = rz * angular_k_
+                  else:
+                      x_box = linear_k_ / abs(rz)
+                      w_box = rz * angular_k_
+
+                # if not np.isclose(yaw, 0., rtol=1e-3, atol=1e-3):
+                #     rospy.logerr(
+                #         'normal at point of contact between %s'
+                #         ' and body is not axis-aligned (wrt %s)',
+                #         robot.name, self._body_frame
+                #     )
+                # else:
+                x_move_base = v if R == np.inf else w * (R - y)
+                w_move_base = w
+
+                magic_k = 0.75
+
+                robot_twist.linear.x = magic_k * x_move_base + (1 - magic_k) * x_box
+                robot_twist.angular.z = magic_k * w_move_base + (1 - magic_k) * w_box
+
+            except (tf2_ros.LookupException,
+                    tf2_ros.ConnectivityException,
+                    tf2_ros.ExtrapolationException) as e:
+                rospy.logwarn(
+                    'cannot lookup transform from %s to %s: %s',
+                    robot.frame, self._body_frame, str(e)
+                )
+                continue
+            
+            robot.cmd_vel.publish(robot_twist)
+
+    def spin(self):
+        rospy.spin()
+
+
+def main():
+    node = SwarmControllerNode()
+    try:
+        node.spin()
+    except rospy.ROSInterruptException:
+        pass
+
+
+if __name__ == '__main__':
+    main()

From acf89b832260eed48d42229b42e46f504c19961e Mon Sep 17 00:00:00 2001
From: Emiliano Borghi <eborghi@ekumenlabs.com>
Date: Fri, 8 Nov 2019 18:29:04 -0300
Subject: [PATCH 2/2] Add changes for Ekuthon 2019 day 2

---
 ca_description/urdf/common_properties.xacro   |    3 +-
 ca_description/urdf/create_2.xacro            |    2 +-
 ca_description/urdf/create_base.xacro         |    7 +-
 ca_description/urdf/create_caster_wheel.xacro |   17 +-
 .../urdf/sensors/cliff_sensors.xacro          |   16 +-
 ca_description/urdf/sensors/wall_sensor.xacro |    4 +-
 .../create_cooperative_two_rooms.launch       |    8 +-
 ca_gazebo/launch/create_empty_world.launch    |    2 +-
 ca_gazebo/worlds/two_rooms.world              | 1071 +----------------
 .../ca_move_base/launch/navigate.launch       |    7 +-
 .../config/dwa_local_planner_params.yaml      |   16 +-
 navigation/ca_swarm/scripts/box_client.py     |   11 +-
 .../ca_swarm/scripts/swarm_controller_node    |  175 +--
 13 files changed, 195 insertions(+), 1144 deletions(-)

diff --git a/ca_description/urdf/common_properties.xacro b/ca_description/urdf/common_properties.xacro
index 8db810ddb..d2e8bad17 100644
--- a/ca_description/urdf/common_properties.xacro
+++ b/ca_description/urdf/common_properties.xacro
@@ -4,8 +4,7 @@
 <xacro:macro name="dummy_inertia">
   <inertial>
     <mass value="0.01" />
-    <origin xyz="0 0 0" />
-    <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
+    <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" />
   </inertial>
 </xacro:macro>
 
diff --git a/ca_description/urdf/create_2.xacro b/ca_description/urdf/create_2.xacro
index 3c33de879..6b6e17cb8 100644
--- a/ca_description/urdf/create_2.xacro
+++ b/ca_description/urdf/create_2.xacro
@@ -17,5 +17,5 @@
   </xacro:create_base>
 
   <!-- Roomblock -->
-  <xacro:roomblock parent="base_link" visualize="$(arg visualize)"/>
+  <!-- <xacro:roomblock parent="base_link" visualize="$(arg visualize)"/> -->
 </robot>
diff --git a/ca_description/urdf/create_base.xacro b/ca_description/urdf/create_base.xacro
index 59e5a1550..1f8df74f7 100644
--- a/ca_description/urdf/create_base.xacro
+++ b/ca_description/urdf/create_base.xacro
@@ -52,7 +52,12 @@
 
     <xacro:create_wheel prefix="left" y_offset="${wheel_separation / 2}" wheel_radius="${wheel_radius}" wheel_width="${wheel_width}"/>
     <xacro:create_wheel prefix="right" y_offset="${wheel_separation / -2}" wheel_radius="${wheel_radius}" wheel_width="${wheel_width}"/>
-    <xacro:caster_wheel/>
+    <xacro:caster_wheel name="caster_front">
+      <origin xyz="0.13 0 0" rpy="0 ${pi/2} ${pi/2}" />
+    </xacro:caster_wheel>
+    <xacro:caster_wheel name="caster_back">
+      <origin xyz="-0.13 0 0" rpy="0 ${pi/2} ${pi/2}" />
+    </xacro:caster_wheel>
 
     <!-- Simulation sensors -->
     <xacro:sim_create_base/>
diff --git a/ca_description/urdf/create_caster_wheel.xacro b/ca_description/urdf/create_caster_wheel.xacro
index 439406ecd..6df1fdb83 100644
--- a/ca_description/urdf/create_caster_wheel.xacro
+++ b/ca_description/urdf/create_caster_wheel.xacro
@@ -1,34 +1,25 @@
 <?xml version="1.0" ?>
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
 
-<xacro:macro name="caster_wheel">
+<xacro:macro name="caster_wheel" params="name *origin">
 
   <xacro:include filename="$(find ca_description)/urdf/common_properties.xacro"/>
 
-  <xacro:property name="link_name" value="caster_wheel_link"/>
+  <xacro:property name="link_name" value="${name}_wheel_link"/>
   <xacro:property name="parent_link" value="base_link"/>
 
-  <xacro:property name="x" value="0.13"/>
-  <xacro:property name="y" value="0"/>
-  <xacro:property name="z" value="0"/>
-  <xacro:property name="R" value="0"/>
-  <xacro:property name="P" value="${pi/2}"/>
-  <xacro:property name="Y" value="${pi/2}"/>
-
   <xacro:property name="radius" value="0.018"/>
 
   <link name="${link_name}">
     <xacro:inertial_sphere mass="0.05" diameter="${2*radius}"/>
 
     <visual>
-      <origin xyz="0 0 0" rpy="${R} ${P} ${Y}" />
       <geometry>
         <sphere radius="${radius}" />
       </geometry>
     </visual>
 
     <collision>
-      <origin xyz="0 0 0" rpy="${R} ${P} ${Y}" />
       <geometry>
         <sphere radius="${radius}" />
       </geometry>
@@ -36,8 +27,8 @@
   </link>
 
   <!-- fixed because there's no transmission -->
-  <joint name="front_castor_joint" type="fixed">
-    <origin xyz="${x} ${y} ${z}" rpy="0 0 0" />
+  <joint name="${name}_joint" type="fixed">
+    <xacro:insert_block name="origin"/>
     <parent link="${parent_link}" />
     <child link="${link_name}" />
     <axis xyz="0 1 0" />
diff --git a/ca_description/urdf/sensors/cliff_sensors.xacro b/ca_description/urdf/sensors/cliff_sensors.xacro
index 36f2db6bd..d70afa9a4 100644
--- a/ca_description/urdf/sensors/cliff_sensors.xacro
+++ b/ca_description/urdf/sensors/cliff_sensors.xacro
@@ -37,21 +37,13 @@
     <child link="${name_front_right}_cliff_sensor_link" />
   </joint>
 
-  <link name="${name_side_left}_cliff_sensor_link">
-    <xacro:dummy_inertia/>
-  </link>
+  <link name="${name_side_left}_cliff_sensor_link"/>
 
-  <link name="${name_side_right}_cliff_sensor_link">
-    <xacro:dummy_inertia/>
-  </link>
+  <link name="${name_side_right}_cliff_sensor_link"/>
 
-  <link name="${name_front_left}_cliff_sensor_link">
-    <xacro:dummy_inertia/>
-  </link>
+  <link name="${name_front_left}_cliff_sensor_link"/>
 
-  <link name="${name_front_right}_cliff_sensor_link">
-    <xacro:dummy_inertia/>
-  </link>
+  <link name="${name_front_right}_cliff_sensor_link"/>
 
   <xacro:sim_create_cliff_sensors visualize="${visualize}"/>
 
diff --git a/ca_description/urdf/sensors/wall_sensor.xacro b/ca_description/urdf/sensors/wall_sensor.xacro
index 01e88f934..52aab61e3 100644
--- a/ca_description/urdf/sensors/wall_sensor.xacro
+++ b/ca_description/urdf/sensors/wall_sensor.xacro
@@ -21,9 +21,7 @@
     <child link="wall_sensor_link" />
   </joint>
 
-  <link name="wall_sensor_link">
-    <xacro:dummy_inertia/>
-  </link>
+  <link name="wall_sensor_link"/>
 
   <xacro:sim_create_wall_sensor visualize="${visualize}"/>
 
diff --git a/ca_gazebo/launch/create_cooperative_two_rooms.launch b/ca_gazebo/launch/create_cooperative_two_rooms.launch
index 6e6bc0b1a..25018a51e 100644
--- a/ca_gazebo/launch/create_cooperative_two_rooms.launch
+++ b/ca_gazebo/launch/create_cooperative_two_rooms.launch
@@ -1,10 +1,10 @@
 <launch>
-  <param name="create1/amcl/initial_pose_x" value="$(optenv X1 5.74)"/>
-  <param name="create1/amcl/initial_pose_y" value="$(optenv Y1 0.96)"/>
+  <param name="create1/amcl/initial_pose_x" value="$(optenv X1 -9.85)"/>
+  <param name="create1/amcl/initial_pose_y" value="$(optenv Y1 1.3)"/>
   <param name="create1/amcl/initial_pose_a" value="$(optenv A1 0)"/>
 
-  <param name="create2/amcl/initial_pose_x" value="$(optenv X2 5.74)"/>
-  <param name="create2/amcl/initial_pose_y" value="$(optenv Y2 0.27)"/>
+  <param name="create2/amcl/initial_pose_x" value="$(optenv X2 -9.85)"/>
+  <param name="create2/amcl/initial_pose_y" value="$(optenv Y2 -0.1)"/>
   <param name="create2/amcl/initial_pose_a" value="$(optenv A2 0)"/>
 
   <node pkg="ca_swarm" type="swarm_controller_node" name="swarm_controller_node" output="screen">
diff --git a/ca_gazebo/launch/create_empty_world.launch b/ca_gazebo/launch/create_empty_world.launch
index 6e2eafb79..17dd791e2 100644
--- a/ca_gazebo/launch/create_empty_world.launch
+++ b/ca_gazebo/launch/create_empty_world.launch
@@ -32,7 +32,7 @@
 	</include>
 
   <!-- map_server -->
-  <include if="$(eval str(arg('localization'))=='amcl')"
+  <include if="$(eval str(arg('localization')) != 'slam')"
 			file="$(find ca_move_base)/launch/map_server.launch">
 	<arg name="env" value="$(arg env)"/>
   </include>
diff --git a/ca_gazebo/worlds/two_rooms.world b/ca_gazebo/worlds/two_rooms.world
index 4130b6757..160502234 100644
--- a/ca_gazebo/worlds/two_rooms.world
+++ b/ca_gazebo/worlds/two_rooms.world
@@ -31,1063 +31,94 @@
       </attenuation>
       <direction>-0.5 0.1 -0.9</direction>
     </light>
-    <model name='rooms'>
-      <model name='ground_plane'>
-        <static>1</static>
-        <link name='link'>
-          <collision name='collision'>
-            <geometry>
-              <plane>
-                <normal>0 0 1</normal>
-                <size>100 100</size>
-              </plane>
-            </geometry>
-            <surface>
-              <friction>
-                <ode>
-                  <mu>100</mu>
-                  <mu2>50</mu2>
-                </ode>
-              </friction>
-              <bounce/>
-              <contact>
-                <ode/>
-              </contact>
-            </surface>
-            <max_contacts>10</max_contacts>
-          </collision>
-          <visual name='visual'>
-            <cast_shadows>0</cast_shadows>
-            <geometry>
-              <plane>
-                <normal>0 0 1</normal>
-                <size>100 100</size>
-              </plane>
-            </geometry>
-            <material>
-              <script>
-                <uri>file://media/materials/scripts/gazebo.material</uri>
-                <name>Gazebo/Grey</name>
-              </script>
-            </material>
-          </visual>
-          <velocity_decay>
-            <linear>0</linear>
-            <angular>0</angular>
-          </velocity_decay>
-          <self_collide>0</self_collide>
-          <kinematic>0</kinematic>
-          <gravity>1</gravity>
-        </link>
-      </model>
-      <model name='door'>
-        <static>0</static>
-        <link name='link'>
-          <inertial>
-            <mass>1</mass>
-            <pose frame=''>0 0 0 0 -0 0</pose>
-          </inertial>
-          <pose frame=''>0.002741 -0.017964 1.2465 0 -0 0</pose>
-          <self_collide>0</self_collide>
-          <enable_wind>0</enable_wind>
-          <kinematic>0</kinematic>
-          <visual name='visual'>
-            <pose frame=''>0 0 0 0 -0 0</pose>
-            <geometry>
-              <box>
-                <size>0.9 0.078144 2.51204</size>
-              </box>
-            </geometry>
-            <material>
-              <lighting>1</lighting>
-              <script>
-                <uri>file://media/materials/scripts/gazebo.material</uri>
-                <name>Gazebo/WoodFloor</name>
-              </script>
-              <shader type='pixel'/>
-              <emissive>0 0 0 1</emissive>
-            </material>
-            <transparency>0</transparency>
-            <cast_shadows>1</cast_shadows>
-          </visual>
-          <collision name='collision'>
-            <laser_retro>0</laser_retro>
-            <max_contacts>10</max_contacts>
-            <pose frame=''>0 0 0 0 -0 0</pose>
-            <geometry>
-              <box>
-                <size>0.9 0.078144 2.51204</size>
-              </box>
-            </geometry>
-            <surface>
-              <friction>
-                <ode>
-                  <mu>0.2</mu>
-                  <mu2>0.2</mu2>
-                  <fdir1>0 0 0</fdir1>
-                  <slip1>0</slip1>
-                  <slip2>0</slip2>
-                </ode>
-                <torsional>
-                  <coefficient>1</coefficient>
-                  <patch_radius>0</patch_radius>
-                  <surface_radius>0</surface_radius>
-                  <use_patch_radius>1</use_patch_radius>
-                  <ode>
-                    <slip>0</slip>
-                  </ode>
-                </torsional>
-              </friction>
-              <bounce>
-                <restitution_coefficient>0</restitution_coefficient>
-                <threshold>1e+06</threshold>
-              </bounce>
-              <contact>
-                <collide_without_contact>0</collide_without_contact>
-                <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
-                <collide_bitmask>1</collide_bitmask>
-                <ode>
-                  <soft_cfm>0</soft_cfm>
-                  <soft_erp>0.2</soft_erp>
-                  <kp>1e+13</kp>
-                  <kd>1</kd>
-                  <max_vel>0.01</max_vel>
-                  <min_depth>0</min_depth>
-                </ode>
-                <bullet>
-                  <split_impulse>1</split_impulse>
-                  <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
-                  <soft_cfm>0</soft_cfm>
-                  <soft_erp>0.2</soft_erp>
-                  <kp>1e+13</kp>
-                  <kd>1</kd>
-                </bullet>
-              </contact>
-            </surface>
-          </collision>
-        </link>
-        <plugin name="model_pose_publisher_plugin" filename="libmodel_pose_publisher_plugin.so"/>
-      </model>
-      <model name='rooms_without_door'>
-        <static>1</static>
-        <link name='Wall_0'>
-          <pose frame=''>-1.425 1.425 -0 0 -0 1.5708</pose>
-          <inertial>
-            <mass>2250</mass>
-            <pose frame=''>0 0 0 0 -0 0</pose>
-            <inertia>
-              <ixx>1176.09</ixx>
-              <ixy>0</ixy>
-              <ixz>0</ixz>
-              <iyy>2859.38</iyy>
-              <iyz>0</iyz>
-              <izz>1691.72</izz>
-            </inertia>
-          </inertial>
-          <self_collide>0</self_collide>
-          <enable_wind>0</enable_wind>
-          <kinematic>0</kinematic>
-          <gravity>1</gravity>
-          <visual name='Wall_0_Visual'>
-            <pose frame=''>0 0 1.25 0 -0 0</pose>
-            <geometry>
-              <box>
-                <size>3 0.15 2.5</size>
-              </box>
-            </geometry>
-            <material>
-              <script>
-                <uri>file://media/materials/scripts/gazebo.material</uri>
-                <name>Gazebo/Bricks</name>
-              </script>
-              <ambient>1 1 1 1</ambient>
-              <shader type='pixel'>
-                <normal_map>__default__</normal_map>
-              </shader>
-              <emissive>0 0 0 1</emissive>
-            </material>
-            <meta>
-              <layer>0</layer>
-            </meta>
-            <transparency>0</transparency>
-            <cast_shadows>1</cast_shadows>
-          </visual>
-          <collision name='Wall_0_Collision'>
-            <laser_retro>0</laser_retro>
-            <max_contacts>10</max_contacts>
-            <pose frame=''>0 0 1.25 0 -0 0</pose>
-            <geometry>
-              <box>
-                <size>3 0.15 2.5</size>
-              </box>
-            </geometry>
-            <surface>
-              <friction>
-                <ode>
-                  <mu>1</mu>
-                  <mu2>1</mu2>
-                  <fdir1>0 0 0</fdir1>
-                  <slip1>0</slip1>
-                  <slip2>0</slip2>
-                </ode>
-                <torsional>
-                  <coefficient>1</coefficient>
-                  <patch_radius>0</patch_radius>
-                  <surface_radius>0</surface_radius>
-                  <use_patch_radius>1</use_patch_radius>
-                  <ode>
-                    <slip>0</slip>
-                  </ode>
-                </torsional>
-              </friction>
-              <bounce>
-                <restitution_coefficient>0</restitution_coefficient>
-                <threshold>1e+06</threshold>
-              </bounce>
-              <contact>
-                <collide_without_contact>0</collide_without_contact>
-                <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
-                <collide_bitmask>1</collide_bitmask>
-                <ode>
-                  <soft_cfm>0</soft_cfm>
-                  <soft_erp>0.2</soft_erp>
-                  <kp>1e+13</kp>
-                  <kd>1</kd>
-                  <max_vel>0.01</max_vel>
-                  <min_depth>0</min_depth>
-                </ode>
-                <bullet>
-                  <split_impulse>1</split_impulse>
-                  <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
-                  <soft_cfm>0</soft_cfm>
-                  <soft_erp>0.2</soft_erp>
-                  <kp>1e+13</kp>
-                  <kd>1</kd>
-                </bullet>
-              </contact>
-            </surface>
-          </collision>
-        </link>
-        <link name='Wall_1'>
-          <pose frame=''>0 2.85 -0 0 -0 0</pose>
-          <inertial>
-            <mass>2250</mass>
-            <pose frame=''>0 0 0 0 -0 0</pose>
-            <inertia>
-              <ixx>1176.09</ixx>
-              <ixy>0</ixy>
-              <ixz>0</ixz>
-              <iyy>2859.38</iyy>
-              <iyz>0</iyz>
-              <izz>1691.72</izz>
-            </inertia>
-          </inertial>
-          <self_collide>0</self_collide>
-          <enable_wind>0</enable_wind>
-          <kinematic>0</kinematic>
-          <gravity>1</gravity>
-          <visual name='Wall_1_Visual'>
-            <pose frame=''>0 0 1.25 0 -0 0</pose>
-            <geometry>
-              <box>
-                <size>3 0.15 2.5</size>
-              </box>
-            </geometry>
-            <material>
-              <script>
-                <uri>file://media/materials/scripts/gazebo.material</uri>
-                <name>Gazebo/Bricks</name>
-              </script>
-              <ambient>1 1 1 1</ambient>
-              <shader type='pixel'>
-                <normal_map>__default__</normal_map>
-              </shader>
-              <emissive>0 0 0 1</emissive>
-            </material>
-            <meta>
-              <layer>0</layer>
-            </meta>
-            <transparency>0</transparency>
-            <cast_shadows>1</cast_shadows>
-          </visual>
-          <collision name='Wall_1_Collision'>
-            <laser_retro>0</laser_retro>
-            <max_contacts>10</max_contacts>
-            <pose frame=''>0 0 1.25 0 -0 0</pose>
-            <geometry>
-              <box>
-                <size>3 0.15 2.5</size>
-              </box>
-            </geometry>
-            <surface>
-              <friction>
-                <ode>
-                  <mu>1</mu>
-                  <mu2>1</mu2>
-                  <fdir1>0 0 0</fdir1>
-                  <slip1>0</slip1>
-                  <slip2>0</slip2>
-                </ode>
-                <torsional>
-                  <coefficient>1</coefficient>
-                  <patch_radius>0</patch_radius>
-                  <surface_radius>0</surface_radius>
-                  <use_patch_radius>1</use_patch_radius>
-                  <ode>
-                    <slip>0</slip>
-                  </ode>
-                </torsional>
-              </friction>
-              <bounce>
-                <restitution_coefficient>0</restitution_coefficient>
-                <threshold>1e+06</threshold>
-              </bounce>
-              <contact>
-                <collide_without_contact>0</collide_without_contact>
-                <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
-                <collide_bitmask>1</collide_bitmask>
-                <ode>
-                  <soft_cfm>0</soft_cfm>
-                  <soft_erp>0.2</soft_erp>
-                  <kp>1e+13</kp>
-                  <kd>1</kd>
-                  <max_vel>0.01</max_vel>
-                  <min_depth>0</min_depth>
-                </ode>
-                <bullet>
-                  <split_impulse>1</split_impulse>
-                  <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
-                  <soft_cfm>0</soft_cfm>
-                  <soft_erp>0.2</soft_erp>
-                  <kp>1e+13</kp>
-                  <kd>1</kd>
-                </bullet>
-              </contact>
-            </surface>
-          </collision>
-        </link>
-        <link name='Wall_2'>
-          <pose frame=''>1.425 1.425 -0 0 0 -1.5708</pose>
-          <inertial>
-            <mass>2250</mass>
-            <pose frame=''>0 0 0 0 -0 0</pose>
-            <inertia>
-              <ixx>1176.09</ixx>
-              <ixy>0</ixy>
-              <ixz>0</ixz>
-              <iyy>2859.38</iyy>
-              <iyz>0</iyz>
-              <izz>1691.72</izz>
-            </inertia>
-          </inertial>
-          <self_collide>0</self_collide>
-          <enable_wind>0</enable_wind>
-          <kinematic>0</kinematic>
-          <gravity>1</gravity>
-          <visual name='Wall_2_Visual'>
-            <pose frame=''>0 0 1.25 0 -0 0</pose>
-            <geometry>
-              <box>
-                <size>3 0.15 2.5</size>
-              </box>
-            </geometry>
-            <material>
-              <script>
-                <uri>file://media/materials/scripts/gazebo.material</uri>
-                <name>Gazebo/Bricks</name>
-              </script>
-              <ambient>1 1 1 1</ambient>
-              <shader type='pixel'>
-                <normal_map>__default__</normal_map>
-              </shader>
-              <emissive>0 0 0 1</emissive>
-            </material>
-            <meta>
-              <layer>0</layer>
-            </meta>
-            <transparency>0</transparency>
-            <cast_shadows>1</cast_shadows>
-          </visual>
-          <collision name='Wall_2_Collision'>
-            <laser_retro>0</laser_retro>
-            <max_contacts>10</max_contacts>
-            <pose frame=''>0 0 1.25 0 -0 0</pose>
-            <geometry>
-              <box>
-                <size>3 0.15 2.5</size>
-              </box>
-            </geometry>
-            <surface>
-              <friction>
-                <ode>
-                  <mu>1</mu>
-                  <mu2>1</mu2>
-                  <fdir1>0 0 0</fdir1>
-                  <slip1>0</slip1>
-                  <slip2>0</slip2>
-                </ode>
-                <torsional>
-                  <coefficient>1</coefficient>
-                  <patch_radius>0</patch_radius>
-                  <surface_radius>0</surface_radius>
-                  <use_patch_radius>1</use_patch_radius>
-                  <ode>
-                    <slip>0</slip>
-                  </ode>
-                </torsional>
-              </friction>
-              <bounce>
-                <restitution_coefficient>0</restitution_coefficient>
-                <threshold>1e+06</threshold>
-              </bounce>
-              <contact>
-                <collide_without_contact>0</collide_without_contact>
-                <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
-                <collide_bitmask>1</collide_bitmask>
-                <ode>
-                  <soft_cfm>0</soft_cfm>
-                  <soft_erp>0.2</soft_erp>
-                  <kp>1e+13</kp>
-                  <kd>1</kd>
-                  <max_vel>0.01</max_vel>
-                  <min_depth>0</min_depth>
-                </ode>
-                <bullet>
-                  <split_impulse>1</split_impulse>
-                  <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
-                  <soft_cfm>0</soft_cfm>
-                  <soft_erp>0.2</soft_erp>
-                  <kp>1e+13</kp>
-                  <kd>1</kd>
-                </bullet>
-              </contact>
-            </surface>
-          </collision>
-        </link>
-        <link name='Wall_3'>
-          <pose frame=''>1.425 -1.425 0 0 0 -1.5708</pose>
-          <inertial>
-            <mass>2250</mass>
-            <pose frame=''>0 0 0 0 -0 0</pose>
-            <inertia>
-              <ixx>1176.09</ixx>
-              <ixy>0</ixy>
-              <ixz>0</ixz>
-              <iyy>2859.38</iyy>
-              <iyz>0</iyz>
-              <izz>1691.72</izz>
-            </inertia>
-          </inertial>
-          <self_collide>0</self_collide>
-          <enable_wind>0</enable_wind>
-          <kinematic>0</kinematic>
-          <gravity>1</gravity>
-          <visual name='Wall_3_Visual'>
-            <pose frame=''>0 0 1.25 0 -0 0</pose>
-            <geometry>
-              <box>
-                <size>3 0.15 2.5</size>
-              </box>
-            </geometry>
-            <material>
-              <script>
-                <uri>file://media/materials/scripts/gazebo.material</uri>
-                <name>Gazebo/Bricks</name>
-              </script>
-              <ambient>1 1 1 1</ambient>
-              <shader type='pixel'>
-                <normal_map>__default__</normal_map>
-              </shader>
-              <emissive>0 0 0 1</emissive>
-            </material>
-            <meta>
-              <layer>0</layer>
-            </meta>
-            <transparency>0</transparency>
-            <cast_shadows>1</cast_shadows>
-          </visual>
-          <collision name='Wall_3_Collision'>
-            <laser_retro>0</laser_retro>
-            <max_contacts>10</max_contacts>
-            <pose frame=''>0 0 1.25 0 -0 0</pose>
-            <geometry>
-              <box>
-                <size>3 0.15 2.5</size>
-              </box>
-            </geometry>
-            <surface>
-              <friction>
-                <ode>
-                  <mu>1</mu>
-                  <mu2>1</mu2>
-                  <fdir1>0 0 0</fdir1>
-                  <slip1>0</slip1>
-                  <slip2>0</slip2>
-                </ode>
-                <torsional>
-                  <coefficient>1</coefficient>
-                  <patch_radius>0</patch_radius>
-                  <surface_radius>0</surface_radius>
-                  <use_patch_radius>1</use_patch_radius>
-                  <ode>
-                    <slip>0</slip>
-                  </ode>
-                </torsional>
-              </friction>
-              <bounce>
-                <restitution_coefficient>0</restitution_coefficient>
-                <threshold>1e+06</threshold>
-              </bounce>
-              <contact>
-                <collide_without_contact>0</collide_without_contact>
-                <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
-                <collide_bitmask>1</collide_bitmask>
-                <ode>
-                  <soft_cfm>0</soft_cfm>
-                  <soft_erp>0.2</soft_erp>
-                  <kp>1e+13</kp>
-                  <kd>1</kd>
-                  <max_vel>0.01</max_vel>
-                  <min_depth>0</min_depth>
-                </ode>
-                <bullet>
-                  <split_impulse>1</split_impulse>
-                  <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
-                  <soft_cfm>0</soft_cfm>
-                  <soft_erp>0.2</soft_erp>
-                  <kp>1e+13</kp>
-                  <kd>1</kd>
-                </bullet>
-              </contact>
-            </surface>
-          </collision>
-        </link>
-        <link name='Wall_4'>
-          <pose frame=''>0 -2.85 0 0 -0 3.14159</pose>
-          <inertial>
-            <mass>2250</mass>
-            <pose frame=''>0 0 0 0 -0 0</pose>
-            <inertia>
-              <ixx>1176.09</ixx>
-              <ixy>0</ixy>
-              <ixz>0</ixz>
-              <iyy>2859.38</iyy>
-              <iyz>0</iyz>
-              <izz>1691.72</izz>
-            </inertia>
-          </inertial>
-          <self_collide>0</self_collide>
-          <enable_wind>0</enable_wind>
-          <kinematic>0</kinematic>
-          <gravity>1</gravity>
-          <visual name='Wall_4_Visual'>
-            <pose frame=''>0 0 1.25 0 -0 0</pose>
-            <geometry>
-              <box>
-                <size>3 0.15 2.5</size>
-              </box>
-            </geometry>
-            <material>
-              <script>
-                <uri>file://media/materials/scripts/gazebo.material</uri>
-                <name>Gazebo/Bricks</name>
-              </script>
-              <ambient>1 1 1 1</ambient>
-              <shader type='pixel'>
-                <normal_map>__default__</normal_map>
-              </shader>
-              <emissive>0 0 0 1</emissive>
-            </material>
-            <meta>
-              <layer>0</layer>
-            </meta>
-            <transparency>0</transparency>
-            <cast_shadows>1</cast_shadows>
-          </visual>
-          <collision name='Wall_4_Collision'>
-            <laser_retro>0</laser_retro>
-            <max_contacts>10</max_contacts>
-            <pose frame=''>0 0 1.25 0 -0 0</pose>
-            <geometry>
-              <box>
-                <size>3 0.15 2.5</size>
-              </box>
-            </geometry>
-            <surface>
-              <friction>
-                <ode>
-                  <mu>1</mu>
-                  <mu2>1</mu2>
-                  <fdir1>0 0 0</fdir1>
-                  <slip1>0</slip1>
-                  <slip2>0</slip2>
-                </ode>
-                <torsional>
-                  <coefficient>1</coefficient>
-                  <patch_radius>0</patch_radius>
-                  <surface_radius>0</surface_radius>
-                  <use_patch_radius>1</use_patch_radius>
-                  <ode>
-                    <slip>0</slip>
-                  </ode>
-                </torsional>
-              </friction>
-              <bounce>
-                <restitution_coefficient>0</restitution_coefficient>
-                <threshold>1e+06</threshold>
-              </bounce>
-              <contact>
-                <collide_without_contact>0</collide_without_contact>
-                <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
-                <collide_bitmask>1</collide_bitmask>
-                <ode>
-                  <soft_cfm>0</soft_cfm>
-                  <soft_erp>0.2</soft_erp>
-                  <kp>1e+13</kp>
-                  <kd>1</kd>
-                  <max_vel>0.01</max_vel>
-                  <min_depth>0</min_depth>
-                </ode>
-                <bullet>
-                  <split_impulse>1</split_impulse>
-                  <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
-                  <soft_cfm>0</soft_cfm>
-                  <soft_erp>0.2</soft_erp>
-                  <kp>1e+13</kp>
-                  <kd>1</kd>
-                </bullet>
-              </contact>
-            </surface>
-          </collision>
-        </link>
-        <link name='Wall_5'>
-          <pose frame=''>-1.425 -1.425 0 0 -0 1.5708</pose>
-          <inertial>
-            <mass>2250</mass>
-            <pose frame=''>0 0 0 0 -0 0</pose>
-            <inertia>
-              <ixx>1176.09</ixx>
-              <ixy>0</ixy>
-              <ixz>0</ixz>
-              <iyy>2859.38</iyy>
-              <iyz>0</iyz>
-              <izz>1691.72</izz>
-            </inertia>
-          </inertial>
-          <self_collide>0</self_collide>
-          <enable_wind>0</enable_wind>
-          <kinematic>0</kinematic>
-          <gravity>1</gravity>
-          <visual name='Wall_5_Visual'>
-            <pose frame=''>0 0 1.25 0 -0 0</pose>
-            <geometry>
-              <box>
-                <size>3 0.15 2.5</size>
-              </box>
-            </geometry>
-            <material>
-              <script>
-                <uri>file://media/materials/scripts/gazebo.material</uri>
-                <name>Gazebo/Bricks</name>
-              </script>
-              <ambient>1 1 1 1</ambient>
-              <shader type='pixel'>
-                <normal_map>__default__</normal_map>
-              </shader>
-              <emissive>0 0 0 1</emissive>
-              <diffuse>0.7 0.7 0.7 1</diffuse>
-              <specular>0.01 0.01 0.01 1</specular>
-            </material>
-            <meta>
-              <layer>0</layer>
-            </meta>
-            <transparency>0</transparency>
-            <cast_shadows>1</cast_shadows>
-          </visual>
-          <collision name='Wall_5_Collision'>
-            <laser_retro>0</laser_retro>
-            <max_contacts>10</max_contacts>
-            <pose frame=''>0 0 1.25 0 -0 0</pose>
-            <geometry>
-              <box>
-                <size>3 0.15 2.5</size>
-              </box>
-            </geometry>
-            <surface>
-              <friction>
-                <ode>
-                  <mu>1</mu>
-                  <mu2>1</mu2>
-                  <fdir1>0 0 0</fdir1>
-                  <slip1>0</slip1>
-                  <slip2>0</slip2>
-                </ode>
-                <torsional>
-                  <coefficient>1</coefficient>
-                  <patch_radius>0</patch_radius>
-                  <surface_radius>0</surface_radius>
-                  <use_patch_radius>1</use_patch_radius>
-                  <ode>
-                    <slip>0</slip>
-                  </ode>
-                </torsional>
-              </friction>
-              <bounce>
-                <restitution_coefficient>0</restitution_coefficient>
-                <threshold>1e+06</threshold>
-              </bounce>
-              <contact>
-                <collide_without_contact>0</collide_without_contact>
-                <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
-                <collide_bitmask>1</collide_bitmask>
-                <ode>
-                  <soft_cfm>0</soft_cfm>
-                  <soft_erp>0.2</soft_erp>
-                  <kp>1e+13</kp>
-                  <kd>1</kd>
-                  <max_vel>0.01</max_vel>
-                  <min_depth>0</min_depth>
-                </ode>
-                <bullet>
-                  <split_impulse>1</split_impulse>
-                  <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
-                  <soft_cfm>0</soft_cfm>
-                  <soft_erp>0.2</soft_erp>
-                  <kp>1e+13</kp>
-                  <kd>1</kd>
-                </bullet>
-              </contact>
-            </surface>
-          </collision>
-        </link>
-        <link name='Wall_6'>
-          <pose frame=''>-1 0 0 0 -0 0</pose>
-          <inertial>
-            <mass>750</mass>
-            <pose frame=''>0 0 0 0 -0 0</pose>
-            <inertia>
-              <ixx>392.031</ixx>
-              <ixy>0</ixy>
-              <ixz>0</ixz>
-              <iyy>453.125</iyy>
-              <iyz>0</iyz>
-              <izz>63.9062</izz>
-            </inertia>
-          </inertial>
-          <self_collide>0</self_collide>
-          <enable_wind>0</enable_wind>
-          <kinematic>0</kinematic>
-          <gravity>1</gravity>
-          <visual name='Wall_6_Visual'>
-            <pose frame=''>0 0 1.25 0 -0 0</pose>
-            <geometry>
-              <box>
-                <size>1 0.15 2.5</size>
-              </box>
-            </geometry>
-            <material>
-              <script>
-                <uri>file://media/materials/scripts/gazebo.material</uri>
-                <name>Gazebo/Bricks</name>
-              </script>
-              <ambient>1 1 1 1</ambient>
-              <shader type='pixel'>
-                <normal_map>__default__</normal_map>
-              </shader>
-              <emissive>0 0 0 1</emissive>
-            </material>
-            <meta>
-              <layer>0</layer>
-            </meta>
-            <transparency>0</transparency>
-            <cast_shadows>1</cast_shadows>
-          </visual>
-          <collision name='Wall_6_Collision'>
-            <laser_retro>0</laser_retro>
-            <max_contacts>10</max_contacts>
-            <pose frame=''>0 0 1.25 0 -0 0</pose>
-            <geometry>
-              <box>
-                <size>1 0.15 2.5</size>
-              </box>
-            </geometry>
-            <surface>
-              <friction>
-                <ode>
-                  <mu>1</mu>
-                  <mu2>1</mu2>
-                  <fdir1>0 0 0</fdir1>
-                  <slip1>0</slip1>
-                  <slip2>0</slip2>
-                </ode>
-                <torsional>
-                  <coefficient>1</coefficient>
-                  <patch_radius>0</patch_radius>
-                  <surface_radius>0</surface_radius>
-                  <use_patch_radius>1</use_patch_radius>
-                  <ode>
-                    <slip>0</slip>
-                  </ode>
-                </torsional>
-              </friction>
-              <bounce>
-                <restitution_coefficient>0</restitution_coefficient>
-                <threshold>1e+06</threshold>
-              </bounce>
-              <contact>
-                <collide_without_contact>0</collide_without_contact>
-                <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
-                <collide_bitmask>1</collide_bitmask>
-                <ode>
-                  <soft_cfm>0</soft_cfm>
-                  <soft_erp>0.2</soft_erp>
-                  <kp>1e+13</kp>
-                  <kd>1</kd>
-                  <max_vel>0.01</max_vel>
-                  <min_depth>0</min_depth>
-                </ode>
-                <bullet>
-                  <split_impulse>1</split_impulse>
-                  <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
-                  <soft_cfm>0</soft_cfm>
-                  <soft_erp>0.2</soft_erp>
-                  <kp>1e+13</kp>
-                  <kd>1</kd>
-                </bullet>
-              </contact>
-            </surface>
-          </collision>
-        </link>
-        <link name='Wall_8'>
-          <pose frame=''>1 -0 0 0 -0 3.14159</pose>
-          <inertial>
-            <mass>750</mass>
-            <pose frame=''>0 0 0 0 -0 0</pose>
-            <inertia>
-              <ixx>392.031</ixx>
-              <ixy>0</ixy>
-              <ixz>0</ixz>
-              <iyy>453.125</iyy>
-              <iyz>0</iyz>
-              <izz>63.9062</izz>
-            </inertia>
-          </inertial>
-          <self_collide>0</self_collide>
-          <enable_wind>0</enable_wind>
-          <kinematic>0</kinematic>
-          <gravity>1</gravity>
-          <visual name='Wall_8_Visual'>
-            <pose frame=''>0 0 1.25 0 -0 0</pose>
-            <geometry>
-              <box>
-                <size>1 0.15 2.5</size>
-              </box>
-            </geometry>
-            <material>
-              <script>
-                <uri>file://media/materials/scripts/gazebo.material</uri>
-                <name>Gazebo/Bricks</name>
-              </script>
-              <ambient>1 1 1 1</ambient>
-              <shader type='pixel'>
-                <normal_map>__default__</normal_map>
-              </shader>
-              <diffuse>0.7 0.7 0.7 1</diffuse>
-              <specular>0.01 0.01 0.01 1</specular>
-              <emissive>0 0 0 1</emissive>
-            </material>
-            <meta>
-              <layer>0</layer>
-            </meta>
-            <transparency>0</transparency>
-            <cast_shadows>1</cast_shadows>
-          </visual>
-          <collision name='Wall_8_Collision'>
-            <laser_retro>0</laser_retro>
-            <max_contacts>10</max_contacts>
-            <pose frame=''>0 0 1.25 0 -0 0</pose>
-            <geometry>
-              <box>
-                <size>1 0.15 2.5</size>
-              </box>
-            </geometry>
-            <surface>
-              <friction>
-                <ode>
-                  <mu>1</mu>
-                  <mu2>1</mu2>
-                  <fdir1>0 0 0</fdir1>
-                  <slip1>0</slip1>
-                  <slip2>0</slip2>
-                </ode>
-                <torsional>
-                  <coefficient>1</coefficient>
-                  <patch_radius>0</patch_radius>
-                  <surface_radius>0</surface_radius>
-                  <use_patch_radius>1</use_patch_radius>
-                  <ode>
-                    <slip>0</slip>
-                  </ode>
-                </torsional>
-              </friction>
-              <bounce>
-                <restitution_coefficient>0</restitution_coefficient>
-                <threshold>1e+06</threshold>
-              </bounce>
-              <contact>
-                <collide_without_contact>0</collide_without_contact>
-                <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
-                <collide_bitmask>1</collide_bitmask>
-                <ode>
-                  <soft_cfm>0</soft_cfm>
-                  <soft_erp>0.2</soft_erp>
-                  <kp>1e+13</kp>
-                  <kd>1</kd>
-                  <max_vel>0.01</max_vel>
-                  <min_depth>0</min_depth>
-                </ode>
-                <bullet>
-                  <split_impulse>1</split_impulse>
-                  <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
-                  <soft_cfm>0</soft_cfm>
-                  <soft_erp>0.2</soft_erp>
-                  <kp>1e+13</kp>
-                  <kd>1</kd>
-                </bullet>
-              </contact>
-            </surface>
-          </collision>
-        </link>
-        <allow_auto_disable>1</allow_auto_disable>
-      </model>
-      <joint name='door_hinge' type='revolute'>
-        <parent>ground_plane::link</parent>
-        <child>door::link</child>
-        <pose frame=''>-0.5 0 0 0 -0 0</pose>
-        <axis>
-          <xyz>0 0 1</xyz>
-          <use_parent_model_frame>true</use_parent_model_frame>
-          <limit>
-            <lower>-3.14</lower>
-            <upper>3.14</upper>
-          </limit>
-          <dynamics>
-            <spring_reference>0</spring_reference>
-            <spring_stiffness>0</spring_stiffness>
-            <damping>0</damping>
-            <friction>0</friction>
-          </dynamics>
-        </axis>
-        <physics>
-          <ode>
-            <limit>
-              <cfm>0</cfm>
-              <erp>0.2</erp>
-            </limit>
-            <suspension>
-              <cfm>0</cfm>
-              <erp>0.2</erp>
-            </suspension>
-          </ode>
-        </physics>
-      </joint>
-    </model>
-    <model name='unit_box'>
-      <pose frame=''>6.42455 0.596341 0.5 0 -0 0</pose>
+    <model name='ground_plane'>
+      <static>1</static>
       <link name='link'>
-        <inertial>
-          <mass>5</mass>
-          <inertia>
-            <ixx>0.166667</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.166667</iyy>
-            <iyz>0</iyz>
-            <izz>0.166667</izz>
-          </inertia>
-          <pose frame=''>0 0 0 0 -0 0</pose>
-        </inertial>
         <collision name='collision'>
           <geometry>
-            <box>
-              <size>1 1 0.5</size>
-            </box>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
           </geometry>
-          <max_contacts>10</max_contacts>
           <surface>
-              <friction>
-                <ode>
-                  <mu>0.7</mu>
-                  <mu2>0.7</mu2>
-                </ode>
-              </friction>
-              <bounce/>
-              <contact>
-                <ode/>
-              </contact>
-            </surface>
+            <friction>
+              <ode>
+                <mu>100</mu>
+                <mu2>50</mu2>
+              </ode>
+            </friction>
+            <bounce/>
+            <contact>
+              <ode/>
+            </contact>
+          </surface>
+          <max_contacts>10</max_contacts>
         </collision>
         <visual name='visual'>
+          <cast_shadows>0</cast_shadows>
           <geometry>
-            <box>
-              <size>1 1 1</size>
-            </box>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
           </geometry>
           <material>
             <script>
-              <name>Gazebo/GreenGlow</name>
               <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
             </script>
           </material>
         </visual>
+        <velocity_decay>
+          <linear>0</linear>
+          <angular>0</angular>
+        </velocity_decay>
         <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
         <kinematic>0</kinematic>
+        <gravity>1</gravity>
       </link>
-      <plugin name="model_pose_publisher_plugin" filename="libmodel_pose_publisher_plugin.so"/>
     </model>
-    <model name='unit_cylinder'>
-      <pose frame=''>-7.49102 -1.46819 0.5 0 -0 0</pose>
+    <model name='unit_box'>
+      <pose frame=''>-9.1 0.6 0.5 0 -0 0</pose>
       <link name='link'>
         <inertial>
           <mass>1</mass>
           <inertia>
-            <ixx>0.145833</ixx>
+            <ixx>0.354167</ixx>
             <ixy>0</ixy>
             <ixz>0</ixz>
-            <iyy>0.145833</iyy>
+            <iyy>0.104167</iyy>
             <iyz>0</iyz>
-            <izz>0.125</izz>
+            <izz>0.416667</izz>
           </inertia>
-          <pose frame=''>0 0 0 0 -0 0</pose>
+          <pose frame=''>0 0 -0.2 0 -0 0</pose>
         </inertial>
         <collision name='collision'>
           <geometry>
-            <cylinder>
-              <radius>0.5</radius>
-              <length>1</length>
-            </cylinder>
+            <box>
+              <size>1 2 0.5</size>
+            </box>
           </geometry>
           <max_contacts>10</max_contacts>
           <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
+              <friction>
+                <ode>
+                  <mu>0.7</mu>
+                  <mu2>0.7</mu2>
+                </ode>
+              </friction>
+              <bounce/>
+              <contact>
                 <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
+              </contact>
+            </surface>
         </collision>
         <visual name='visual'>
           <geometry>
-            <cylinder>
-              <radius>0.5</radius>
-              <length>1</length>
-            </cylinder>
+            <box>
+              <size>1 2 0.5</size>
+            </box>
           </geometry>
           <material>
             <script>
diff --git a/navigation/ca_move_base/launch/navigate.launch b/navigation/ca_move_base/launch/navigate.launch
index 87dcd3eb9..c46ad7b22 100644
--- a/navigation/ca_move_base/launch/navigate.launch
+++ b/navigation/ca_move_base/launch/navigate.launch
@@ -5,7 +5,7 @@
 	<include file="$(find ca_bumper2pc)/launch/standalone.launch"/>
 
 	<!-- move_base -->
-	<include file="$(find ca_move_base)/launch/move_base.launch">
+	<include if="$(eval str(arg('localization')) != 'slam')" file="$(find ca_move_base)/launch/move_base.launch">
     <arg name="ns" value="$(arg robot_name)"/>
   </include>
 
@@ -13,6 +13,11 @@
   <include if="$(eval str(arg('localization')) == 'amcl')" file="$(find ca_move_base)/launch/amcl.launch">
     <arg name="ns" value="$(arg robot_name)"/>
   </include>
+  
+  <node name="map_static_tf1" pkg="tf" type="static_transform_publisher" 
+        args="0 0 0 0 0 0 1 map /create1_tf/odom 50"/>
+  <node name="map_static_tf2" pkg="tf" type="static_transform_publisher" 
+        args="0 0 0 0 0 0 1 map /create2_tf/odom 50"/>
 
   <!-- slam_gmapping -->
   <include if="$(eval str(arg('localization')) == 'slam')" file="$(find ca_slam)/launch/slam_gmapping.launch">
diff --git a/navigation/ca_swarm/config/dwa_local_planner_params.yaml b/navigation/ca_swarm/config/dwa_local_planner_params.yaml
index ace8482eb..71966bea4 100644
--- a/navigation/ca_swarm/config/dwa_local_planner_params.yaml
+++ b/navigation/ca_swarm/config/dwa_local_planner_params.yaml
@@ -2,30 +2,30 @@ DWAPlannerROS:
 
 # Robot Configuration Parameters - Kobuki
   max_vel_x: 0.3
-  min_vel_x: 0.0
+  min_vel_x: 0.15
 
   max_vel_y: 0.0  # diff drive robot
   min_vel_y: 0.0  # diff drive robot
 
-  max_trans_vel: 0.25 # choose slightly less than the base's capability
-  min_trans_vel: 0.1  # this is the min trans velocity when there is negligible rotational velocity
+  max_trans_vel: 0.3 # choose slightly less than the base's capability
+  min_trans_vel: 0.15  # this is the min trans velocity when there is negligible rotational velocity
   trans_stopped_vel: 0.1
 
   # Warning!
   #   do not set min_trans_vel to 0.0 otherwise dwa will always think translational velocities
   #   are non-negligible and small in place rotational velocities will be created.
 
-  max_rot_vel: 1.5  # choose slightly less than the base's capability
-  min_rot_vel: 0.4  # this is the min angular velocity when there is negligible translational velocity
-  rot_stopped_vel: 0.4
+  max_rot_vel: 0.5  # choose slightly less than the base's capability
+  min_rot_vel: 0.1  # this is the min angular velocity when there is negligible translational velocity
+  rot_stopped_vel: 0.1
 
   acc_lim_x: 0.8
   acc_lim_theta: 2.0
   acc_lim_y: 0.0      # diff drive robot
 
 # Goal Tolerance Parameters
-  yaw_goal_tolerance: 0.3  # 18 degrees
-  xy_goal_tolerance: 0.15  # 15 cm
+  yaw_goal_tolerance: 0.5  # 18 degrees
+  xy_goal_tolerance: 0.4  # 15 cm
   # latch_xy_goal_tolerance: false
 
 # Forward Simulation Parameters
diff --git a/navigation/ca_swarm/scripts/box_client.py b/navigation/ca_swarm/scripts/box_client.py
index 1a45629da..84fe07a17 100755
--- a/navigation/ca_swarm/scripts/box_client.py
+++ b/navigation/ca_swarm/scripts/box_client.py
@@ -20,7 +20,11 @@ def __init__(self):
 
         # Append each of the four waypoints to the list.  Each waypoint
         # is a pose consisting of a position and orientation in the map frame.
-        waypoints.append(Pose(Point(10.0, 0.62, 0.0), Quaternion(0,0,0,1)))
+        waypoints.append(Pose(Point(-4, 0.5, 0), Quaternion(0, 0, 0.216, -0.976)))
+        waypoints.append(Pose(Point(0, 1, 0), Quaternion(0, 0, 0.216, 0.976)))
+
+        # Publisher to manually control the robot (e.g. to stop it)
+        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
 
         # Initialize the visualization markers for RViz
         self.init_markers()
@@ -83,6 +87,9 @@ def move(self, goal):
                 state = self.move_base.get_state()
                 if state == GoalStatus.SUCCEEDED:
                     rospy.loginfo("Goal succeeded!")
+                    # Stop the robot
+                    self.cmd_vel_pub.publish(Twist())
+                    rospy.sleep(1)
 
     def init_markers(self):
         # Set up our waypoint markers
@@ -116,7 +123,7 @@ def init_markers(self):
     def shutdown(self):
         rospy.loginfo("Stopping the robot...")
         # Cancel any active goals
-        self.move_base.cancel_goal()
+        # self.move_base.cancel_goal()
         rospy.sleep(2)
 
 if __name__ == '__main__':
diff --git a/navigation/ca_swarm/scripts/swarm_controller_node b/navigation/ca_swarm/scripts/swarm_controller_node
index a547bd807..c4acf9439 100755
--- a/navigation/ca_swarm/scripts/swarm_controller_node
+++ b/navigation/ca_swarm/scripts/swarm_controller_node
@@ -9,14 +9,15 @@ import tf2_ros
 from tf import transformations
 
 from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
 
 class SwarmRobotProxy(object):
 
-    def __init__(self, name, frame, cmd_vel, initial_tf):
+    def __init__(self, name, frame, cmd_vel, b_T_i):
         self.name = name
         self.frame = frame
         self.cmd_vel = cmd_vel
-        self.initial_tf = initial_tf
+        self.b_T_i = b_T_i
 
 class SwarmControllerNode(object):
 
@@ -32,10 +33,17 @@ class SwarmControllerNode(object):
                 cmd_vel=rospy.Publisher(
                     robot_name + '/cmd_vel', Twist, queue_size=10
                 ),
-                initial_tf=transformations.identity_matrix()
+                b_T_i=transformations.identity_matrix()
             ) for robot_name in rospy.get_param('~swarm_robots')
         ]
 
+        self._box_v = 0.
+        self._box_w = 0.
+        rospy.Subscriber("/unit_box/odom", Odometry, self.box_odometry)
+
+        self._v0 = 0.
+        self._w0 = 0.
+
         self._tf_buffer = tf2_ros.Buffer()
         self._tf_listener = tf2_ros.TransformListener(self._tf_buffer)
         self._body_frame = rospy.get_param('~body_frame')
@@ -44,83 +52,98 @@ class SwarmControllerNode(object):
             'cmd_vel', Twist, self._on_swarm_cmd_vel
         )
 
+        self._dt = 0.1
+        rospy.Timer(rospy.Duration(self._dt), self.timer_cb) # 20 Hz
+
+    def timer_cb(self, event):
+      self.run()
+    
     def _on_swarm_cmd_vel(self, swarm_twist):
-        v = swarm_twist.linear.x
-        w = swarm_twist.angular.z
-        R = v / w if w != 0. else np.inf
-        for robot in self._swarm_robots:
-            robot_twist = Twist()
-            try:
-                robot_transform_in_body_frame = self._tf_buffer.lookup_transform(
-                    self._body_frame, robot.frame, rospy.Time()
-                ).transform
-
-                x = robot_transform_in_body_frame.translation.x
-                y = robot_transform_in_body_frame.translation.y
-                _, _, yaw = transformations.euler_from_quaternion([
-                    robot_transform_in_body_frame.rotation.x,
-                    robot_transform_in_body_frame.rotation.y,
-                    robot_transform_in_body_frame.rotation.z,
-                    robot_transform_in_body_frame.rotation.w
-                ])
-
-                if np.allclose(robot.initial_tf, np.identity(4, dtype=np.float64)):
-                  robot.initial_tf = transformations.compose_matrix(angles=[0., 0., yaw], translate=[x, y, 0.])
-                else:
-                  new_tf = transformations.compose_matrix(angles=[0, 0, yaw], translate=[x, y, 0])
-                  tf_error = transformations.concatenate_matrices(
-                    transformations.inverse_matrix(robot.initial_tf),
-                    new_tf
-                  )
-                  [tx, ty, _] = transformations.translation_from_matrix(tf_error)
-                  [_, _, rz] = transformations.euler_from_matrix(tf_error)
-
-
-                  linear_k_ = 0.02 # Scale linear velocity, chosen by trial and error
-                  angular_k_ = 0.08 # Scale angular velocity, chosen by trial and error
-                  # Bad readings, stop
-                  if np.isclose(rz, 0., rtol=1e-3, atol=1e-3) and \
-                     np.isclose(tx, 0., rtol=1e-3, atol=1e-3) and \
-                     np.isclose(ty, 0., rtol=1e-3, atol=1e-3):
-                      x_box = 0
-                      w_box = 0
-                  elif np.isclose(tx, 0., rtol=1e-3, atol=1e-3) and \
-                       np.isclose(ty, 0., rtol=1e-3, atol=1e-3):
-                      # Too close, just rotate
-                      x_box = 0
-                      w_box = rz * angular_k_
-                  else:
-                      x_box = linear_k_ / abs(rz)
-                      w_box = rz * angular_k_
-
-                # if not np.isclose(yaw, 0., rtol=1e-3, atol=1e-3):
-                #     rospy.logerr(
-                #         'normal at point of contact between %s'
-                #         ' and body is not axis-aligned (wrt %s)',
-                #         robot.name, self._body_frame
-                #     )
-                # else:
-                x_move_base = v if R == np.inf else w * (R - y)
-                w_move_base = w
-
-                magic_k = 0.75
-
-                robot_twist.linear.x = magic_k * x_move_base + (1 - magic_k) * x_box
-                robot_twist.angular.z = magic_k * w_move_base + (1 - magic_k) * w_box
-
-            except (tf2_ros.LookupException,
-                    tf2_ros.ConnectivityException,
-                    tf2_ros.ExtrapolationException) as e:
-                rospy.logwarn(
-                    'cannot lookup transform from %s to %s: %s',
-                    robot.frame, self._body_frame, str(e)
-                )
-                continue
-            
-            robot.cmd_vel.publish(robot_twist)
+        self._v0 = swarm_twist.linear.x
+        self._w0 = swarm_twist.angular.z
+    
+    def box_odometry(self, msg):
+        self._box_v = msg.twist.twist.linear.x
+        self._box_w = msg.twist.twist.angular.z
 
     def spin(self):
         rospy.spin()
+    
+    def run(self):
+      R_desired = 2
+
+      v = min(max(self._v0, R_desired * self._w0), 0.3)
+      w = min(v/R_desired, self._w0)
+
+      R = v / w if w != 0. else np.inf
+
+      self._box_error_v = v - self._box_v
+      self._box_error_w = w - self._box_w
+
+      for robot in self._swarm_robots:
+          robot_twist = Twist()
+          try:
+              robot_transform_in_body_frame = self._tf_buffer.lookup_transform(
+                  self._body_frame, robot.frame, rospy.Time()
+              ).transform
+
+              x = robot_transform_in_body_frame.translation.x
+              y = robot_transform_in_body_frame.translation.y
+              _, _, yaw = transformations.euler_from_quaternion([
+                  robot_transform_in_body_frame.rotation.x,
+                  robot_transform_in_body_frame.rotation.y,
+                  robot_transform_in_body_frame.rotation.z,
+                  robot_transform_in_body_frame.rotation.w
+              ])
+
+              v_box = 0.
+              w_box = 0.
+
+              if np.allclose(robot.b_T_i, np.identity(4, dtype=np.float64)):
+                robot.b_T_i = transformations.compose_matrix(angles=[0., 0., yaw], translate=[x, y, 0.])
+              else:
+                b_T_c = transformations.compose_matrix(angles=[0, 0, yaw], translate=[x, y, 0])
+                
+                K = 0.65
+                box_mass = 1 # kg
+
+                dv = self._box_error_v if R == np.inf else self._box_error_w * (R - y)
+                i_T_tau = transformations.compose_matrix(translate=[max(0, dv * box_mass / K), 0., 0])
+                
+                c_T_tau = transformations.concatenate_matrices(
+                  transformations.inverse_matrix(b_T_c),
+                  robot.b_T_i,
+                  i_T_tau
+                )
+
+                # https://globaljournals.org/GJRE_Volume14/1-Kinematics-Localization-and-Control.pdf
+                [xd, yd, _] = transformations.translation_from_matrix(c_T_tau)
+                [_, _, beta] = transformations.euler_from_matrix(c_T_tau)
+                
+                rho = np.linalg.norm([xd, yd])
+                theta = np.arctan2(yd, xd)
+
+                K_theta = 1.0
+                K_rho = 1.2
+                K_alpha = 0.5
+
+                v_box = K_rho * rho * np.cos(theta)
+                w_box = K_theta * theta * rho + K_alpha * beta
+
+              # Send velocities to the robots
+              robot_twist.linear.x = v_box
+              robot_twist.angular.z = w_box
+
+          except (tf2_ros.LookupException,
+                  tf2_ros.ConnectivityException,
+                  tf2_ros.ExtrapolationException) as e:
+              rospy.logwarn(
+                  'cannot lookup transform from %s to %s: %s',
+                  robot.frame, self._body_frame, str(e)
+              )
+              continue
+          
+          robot.cmd_vel.publish(robot_twist)
 
 
 def main():