trajectory_tracker: add a mode to apply the same control method durin…

…g turning in place (#513)

trajectory_tracker/README.md

@@ -48,9 +48,9 @@ trajectory_tracker node controls vehicle velocity to follow given path.
 * "max_acc" (double, default: 1.0)
 * "max_angacc" (double, default: 2.0)
 * "acc_toc_factor" (double, default: 0.9)
-  > decrease max_acc by this factor in time optimal control to reduce vibration due to control delay
+  > decrease max_acc by this factor in time optimal control to reduce vibration due to control delay.
 * "angacc_toc_factor" (double, default: 0.9)
-  > decrease max_angacc by this factor in time optimal control to reduce vibration due to control delay
+  > decrease max_angacc by this factor in time optimal control to reduce vibration due to control delay. This parameter is valid when "use_time_optimal_control" is true.
 * "path_step" (int, default: 1)
 * "goal_tolerance_dist" (double, default: 0.2)
 * "goal_tolerance_ang" (double, default: 0.1)
@@ -67,6 +67,14 @@ trajectory_tracker node controls vehicle velocity to follow given path.
   > If true, predicted coordinates of the robot at the present timestamp are used. This parameter is valid when "use_odom" is true.
 * "odom_timeout_sec" (double, default: 0.1)
   > Robot will be stopped after the duration specified in this parameter has passed since the last odometry was received. This parameter is valid when "use_odom" is true.
+* "use_time_optimal_control" (bool, default: True)
+  > If true, time optimal control mode is used during turning in place. Otherwise, the same algorithm used for path tracking is used.
+* "time_optimal_control_future_gain" (double, default: 1.5)
+  > A gain to look ahead to robot's angle used in time optimal control. This parameter is valid when "use_time_optimal_control" is true. 
+* "k_ang_rotation" (double, default: 1.0)
+  > "k_ang" value used during turning in place. This parameter is valid when "use_time_optimal_control" is false.
+* "k_avel_rotation" (double, default: 1.0)
+  > "k_avvel" value used during turning in place. This parameter is valid when "use_time_optimal_control" is false.
 
 ----
 

trajectory_tracker/cfg/TrajectoryTracker.cfg

@@ -31,5 +31,9 @@ gen.add("allow_backward", bool_t, 0, "", True)
 gen.add("limit_vel_by_avel", bool_t, 0, "", False)
 gen.add("check_old_path", bool_t, 0, "", False)
 gen.add("epsilon", double_t, 0, "", 0.001, 0.0, 10.0)
+gen.add("use_time_optimal_control", bool_t, 0, "", True)
+gen.add("time_optimal_control_future_gain", double_t, 0, "", 1.5, 0.0, 100.0)
+gen.add("k_ang_rotation", double_t, 0, "", 1.0, 0.0, 100.0)
+gen.add("k_avel_rotation", double_t, 0, "", 1.0, 0.0, 100.0)
 
 exit(gen.generate(PACKAGE, "trajectory_tracker", "TrajectoryTracker"))

trajectory_tracker/src/trajectory_tracker.cpp

@@ -112,6 +112,10 @@ class TrackerNode
   bool check_old_path_;
   double epsilon_;
   double max_dt_;
+  bool use_time_optimal_control_;
+  double time_optimal_control_future_gain_;
+  double k_ang_rotation_;
+  double k_avel_rotation_;
 
   ros::Subscriber sub_path_;
   ros::Subscriber sub_path_velocity_;
@@ -255,6 +259,10 @@ void TrackerNode::cbParameter(const TrajectoryTrackerConfig& config, const uint3
   limit_vel_by_avel_ = config.limit_vel_by_avel;
   check_old_path_ = config.check_old_path;
   epsilon_ = config.epsilon;
+  use_time_optimal_control_ = config.use_time_optimal_control;
+  time_optimal_control_future_gain_ = config.time_optimal_control_future_gain;
+  k_ang_rotation_ = config.k_ang_rotation;
+  k_avel_rotation_ = config.k_avel_rotation;
 }
 
 TrackerNode::~TrackerNode()
@@ -440,7 +448,8 @@ void TrackerNode::spin()
 
 void TrackerNode::control(
     const tf2::Stamped<tf2::Transform>& robot_to_odom,
-    const Eigen::Vector3d& prediction_offset, const double dt)
+    const Eigen::Vector3d& prediction_offset,
+    const double dt)
 {
   trajectory_tracker_msgs::TrajectoryTrackerStatus status;
   status.header.stamp = ros::Time::now();
@@ -471,9 +480,19 @@ void TrackerNode::control(
       if (tracking_result.turning_in_place)
       {
         v_lim_.set(0.0, tracking_result.target_linear_vel, acc_[0], dt);
-        const double target_anglar_vel = tracking_result.angle_remains + w_lim_.get() * dt * 1.5;
-        w_lim_.set(trajectory_tracker::timeOptimalControl(target_anglar_vel, acc_toc_[1]), vel_[1], acc_[1], dt);
 
+        if (use_time_optimal_control_)
+        {
+          const double expected_angle_remains =
+              tracking_result.angle_remains + w_lim_.get() * dt * time_optimal_control_future_gain_;
+          w_lim_.set(trajectory_tracker::timeOptimalControl(expected_angle_remains, acc_toc_[1]), vel_[1], acc_[1], dt);
+        }
+        else
+        {
+          const double wvel_increment =
+              (-tracking_result.angle_remains * k_ang_rotation_ - w_lim_.get() * k_avel_rotation_) * dt;
+          w_lim_.increment(wvel_increment, vel_[1], acc_[1], dt);
+        }
         ROS_DEBUG(
             "trajectory_tracker: angular residual %0.3f, angular vel %0.3f",
             tracking_result.angle_remains, w_lim_.get());

trajectory_tracker/test/CMakeLists.txt

@@ -17,7 +17,12 @@ target_link_libraries(test_trajectory_tracker ${catkin_LIBRARIES} ${Boost_LIBRAR
 add_dependencies(test_trajectory_tracker trajectory_tracker)
 
 add_rostest(test/trajectory_tracker_rostest.test
-  ARGS odom_delay:=0.04 use_odom:=true
+  ARGS odom_delay:=0.04 use_odom:=true use_time_optimal_control:=true
+  DEPENDENCIES test_trajectory_tracker
+)
+
+add_rostest(test/trajectory_tracker_rostest.test
+  ARGS odom_delay:=0.04 use_odom:=true use_time_optimal_control:=false
   DEPENDENCIES test_trajectory_tracker
 )
 

trajectory_tracker/test/test/trajectory_tracker_rostest.test

@@ -2,8 +2,9 @@
 <launch>
   <arg name="odom_delay" default="0.0" />
   <arg name="use_odom" default="false" />
+  <arg name="use_time_optimal_control" default="true" />
 
-  <env name="GCOV_PREFIX" value="/tmp/gcov/trajectory_tracker_d$(arg odom_delay)_$(arg use_odom)" />
+  <env name="GCOV_PREFIX" value="/tmp/gcov/trajectory_tracker_d$(arg odom_delay)_$(arg use_odom)_$(arg use_time_optimal_control)" />
 
   <param name="neonavigation_compatible" value="1" />
   <param name="use_sim_time" value="true" /><!-- clock is provided by the test node -->
@@ -31,5 +32,8 @@
     <param name="dist_lim" value="0.5" />
     <param name="use_odom" value="$(arg use_odom)" />
     <param name="odom_timeout_sec" value="0.0" />
+    <param name="use_time_optimal_control" value="$(arg use_time_optimal_control)" />
+    <param name="k_ang_rotation" value="8.0" />
+    <param name="k_avel_rotation" value="5.0" />
   </node>
 </launch>