From e2bc57defc1f1518126a0cb91b6a485041cc2029 Mon Sep 17 00:00:00 2001
From: Ian Chen <ichen@osrfoundation.org>
Date: Tue, 14 Apr 2020 17:21:05 -0700
Subject: [PATCH 1/2] tune controller

---
 src/subt_seed_node.cc | 31 ++++++++++++++++---------------
 1 file changed, 16 insertions(+), 15 deletions(-)

diff --git a/src/subt_seed_node.cc b/src/subt_seed_node.cc
index 48bd6cd..33dfb8e 100644
--- a/src/subt_seed_node.cc
+++ b/src/subt_seed_node.cc
@@ -140,7 +140,7 @@ void Controller::Update()
 
       // Create a cmd_vel publisher to control a vehicle.
       this->originClient = this->n.serviceClient<subt_msgs::PoseFromArtifact>(
-          "/subt/pose_from_artifact_origin");
+          "/subt/pose_from_artifact_origin", true);
       this->originSrv.request.robot_name.data = this->name;
     }
     else
@@ -231,9 +231,11 @@ not available.");
     double cosy_cosp = +1.0 - 2.0 * (q.y * q.y + q.z * q.z);
     auto yaw = atan2(siny_cosp, cosy_cosp);
 
+    auto yaw90 = M_PI * 0.5;
+
     auto facingFront = abs(yaw) < 0.1;
-    auto facingEast = abs(yaw + M_PI * 0.5) < 0.1;
-    auto facingWest = abs(yaw - M_PI * 0.5) < 0.1;
+    auto facingEast = abs(yaw + yaw90) < 0.1;
+    auto facingWest = abs(yaw - yaw90) < 0.1;
 
     auto onCenter = abs(pose.position.y) <= 1.0;
     auto westOfCenter = pose.position.y > 1.0;
@@ -248,28 +250,27 @@ not available.");
       msg.linear.x = linVel;
       msg.angular.z = angVel * -yaw;
     }
-    // Turn to center line
-    else if (!facingEast && westOfCenter)
-    {
-      msg.angular.z = -angVel;
-    }
-    else if (!facingWest && eastOfCenter)
+    // Center line, not facing entrance
+    else if (!facingFront && onCenter)
     {
-      msg.angular.z = angVel;
+      msg.angular.z = angVel * -yaw;
     }
     // Go to center line
     else if (facingEast && westOfCenter)
     {
-      msg.linear.x = linVel;
+      msg.linear.x = linVel * std::min(pose.position.y, 1.0);
     }
     else if (facingWest && eastOfCenter)
     {
-      msg.linear.x = linVel;
+      msg.linear.x = linVel * std::min(-pose.position.y, 1.0);
     }
-    // Center line, not facing entrance
-    else if (onCenter && !facingFront)
+    else if (westOfCenter)
     {
-      msg.angular.z = angVel * -yaw;
+      msg.angular.z = angVel * (-yaw90 - yaw);
+    }
+    else if (eastOfCenter)
+    {
+      msg.angular.z = angVel * (yaw90 - yaw);
     }
     else
     {

From 3a84931c647322aa107685cca7610a3a69facdfc Mon Sep 17 00:00:00 2001
From: Ian Chen <ichen@osrfoundation.org>
Date: Tue, 14 Apr 2020 18:10:48 -0700
Subject: [PATCH 2/2] dockerfile changes

---
 docker/subt_solution/Dockerfile | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/docker/subt_solution/Dockerfile b/docker/subt_solution/Dockerfile
index 3ce2e53..d837174 100644
--- a/docker/subt_solution/Dockerfile
+++ b/docker/subt_solution/Dockerfile
@@ -15,7 +15,6 @@ RUN apt-get update -qq \
         libspnav-dev \
         libusb-dev \
         lsb-release \
-        mercurial \
         python3-dbg \
         python3-empy \
         python3-numpy \
@@ -44,6 +43,7 @@ RUN /bin/sh -c 'echo "deb [trusted=yes] http://packages.ros.org/ros/ubuntu $(lsb
  && apt-get update -qq \
  && apt-get install -y -qq \
     python-catkin-tools \
+    python-rosdep \
     python-rosinstall \
     ros-melodic-desktop \
     ros-melodic-joystick-drivers \
@@ -94,8 +94,8 @@ RUN rosdep update
 
 RUN mkdir -p subt_solution/src \
  && cd subt_solution/src \
- && hg clone https://bitbucket.org/osrf/subt -b urban_circuit \
- && hg clone https://bitbucket.org/osrf/subt_seed
+ && git clone https://github.com/osrf/subt.git -b urban_circuit \
+ && git clone https://github.com/osrf/subt_seed.git
 
 WORKDIR /home/$USERNAME/subt_solution