From d2492256bc1e1e42fae183c8f541223721af592b Mon Sep 17 00:00:00 2001
From: Martin Dlouhy <md@robotika.cz>
Date: Fri, 24 Sep 2021 21:55:19 -0400
Subject: [PATCH] Skiddy pre-finals tweaks (#936)

* traversability.cc - use waitForTransform()

* osgar/drivers/cortexpilot.py - hack, disable oscillate and limit turn to 15deg/sec

* disable statistics and save rosbag

* subt/script/run_skiddy.bash - parameters used for finals
---
 osgar/drivers/cortexpilot.py        |  4 +++-
 subt/ros/proxy/traversability.cc    | 18 +++++++++++++++---
 subt/ros/robot/launch/skiddy.launch |  4 +---
 subt/script/run_skiddy.bash         |  4 ++--
 4 files changed, 21 insertions(+), 9 deletions(-)

diff --git a/osgar/drivers/cortexpilot.py b/osgar/drivers/cortexpilot.py
index adde5918b..183425334 100644
--- a/osgar/drivers/cortexpilot.py
+++ b/osgar/drivers/cortexpilot.py
@@ -20,7 +20,7 @@
 RAMP_STEP = 0.1  # fractional number for speed in -1.0 .. 1.0
 
 SOFT_SPEED_LIMIT = 1.0  # m/s ... software speed limit, abs() value
-SOFT_ANGULAR_SPEED_LIMIT = math.radians(45)  # rad/s
+SOFT_ANGULAR_SPEED_LIMIT = math.radians(15)  # rad/s
 
 
 def sint32_diff(a, b):
@@ -310,12 +310,14 @@ def process_packet(self, packet):
 
     def on_desired_speed(self, data):
         self.desired_speed, self.desired_angular_speed = data[0] / 1000.0, math.radians(data[1] / 100.0)
+        """
         if abs(self.desired_speed) < 0.2 and abs(self.desired_angular_speed) > 0.2:
             if self.speeds.__name__ != "oscilate":
                 self.speeds = self.oscilate()
         else:
             if self.speeds.__name__ == "oscilate":
                 self.speeds = self.plain_speeds()
+                """
         self.cmd_flags |= 0x02  # PWM ON
 
     def run(self):
diff --git a/subt/ros/proxy/traversability.cc b/subt/ros/proxy/traversability.cc
index f552ba9e0..813cd09f3 100644
--- a/subt/ros/proxy/traversability.cc
+++ b/subt/ros/proxy/traversability.cc
@@ -259,7 +259,7 @@ std::optional<tf::StampedTransform> Traversability::GetTransform(
 {
   tf::StampedTransform pose;
   tf::TransformException error("none");
-  for (int i = 0; i < 40; ++i)
+  /*for (int i = 0; i < 40; ++i)
   {
     try
     {
@@ -271,8 +271,20 @@ std::optional<tf::StampedTransform> Traversability::GetTransform(
       error = e;
       ros::Duration(0.005).sleep();
     }
-  }
-  ROS_ERROR("%s", error.what());
+  }*/
+
+    try
+    {
+      transform_listener_.waitForTransform(target_frame, source_frame, when, ros::Duration(0.2));
+      transform_listener_.lookupTransform(target_frame, source_frame, when, pose);
+      return pose;
+    }
+    catch (tf::TransformException& e)
+    {
+      error = e;
+      ros::Duration(0.005).sleep();
+    }
+  ROS_ERROR("MD-%s", error.what());
   return {};
 }
 
diff --git a/subt/ros/robot/launch/skiddy.launch b/subt/ros/robot/launch/skiddy.launch
index f547e9523..83acb3869 100644
--- a/subt/ros/robot/launch/skiddy.launch
+++ b/subt/ros/robot/launch/skiddy.launch
@@ -1,7 +1,5 @@
 <?xml version="1.0"?>
 <launch>
-  <param name="enable_statistics" value="true"/>
-
   <!-- Announces position of the point tracked by tracking camera relative to the center of the robot. -->
   <node pkg="tf" type="static_transform_publisher" name="tracked_point_tf" args="0.282 0 0.33 0 0 0 global odom 100" />
   <!-- Announces the position of the center of the robot relative to the point tracked by tracking camera in Osgar.
@@ -89,6 +87,6 @@
     <param name="output_pointcloud_channel" value="points"/>
   </node>
 
-  <node name="record" pkg="rosbag" type="record" args="-a -o /tmp/skiddy"/>
+  <node name="record" pkg="rosbag" type="record" args="-a -o skiddy"/>
 
 </launch>
diff --git a/subt/script/run_skiddy.bash b/subt/script/run_skiddy.bash
index d8b3e9d3f..4b4bad525 100755
--- a/subt/script/run_skiddy.bash
+++ b/subt/script/run_skiddy.bash
@@ -5,6 +5,6 @@ export PYTHONPATH=${PWD}:${PYTHONPATH}
 export OMP_NUM_THREADS=1  # Make OpenCV eat fewer cpus.
 python3 -m osgar.record config/skiddy-subt-serialloop.json &
 pidcore=$!
-python3 -m subt.main --use-old-record run config/skiddy-subt.json --side left --speed 0.4 --timeout 180 --wall-dist 1.0 --gap-size 1.0
+python3 -m subt.main --use-old-record run config/skiddy-subt.json --side right --speed 0.4 --timeout 900 --wall-dist 0.6 --gap-size 0.6 --init-path "15.0,0"
 #python3 -m osgar.go run config/skiddy-subt.json -s 0.3 -d -4 --timeout 30
-kill "$pidcore"
\ No newline at end of file
+kill "$pidcore"